public float Multiply(LineSeg3D ls1, LineSeg3D ls2)
{
Vector3D d1 = new Vector3D(ls1._ep - ls1._sp);
Vector3D d2 = new Vector3D(ls2._ep - ls2._sp);
return(d1.GetX() * d2.GetX() + d1.GetY() * d2.GetY() + d1.GetZ() * d2.GetZ());
}
/// <summary>
/// Note that the axis doesn't have to be a unit vector.
/// </summary>
/// <param name="axis">the axis around which to rotate</param>
/// <param name="angle">the angle of rotation in radians</param>
public void Set(Vector3D axis, float angle)
{
// start with an identity matrix.
this.SetIdentity();
// Identity is all that can be deduced from zero angle
if (angle == 0)
{
return;
}
// normalize to a unit vector u
float[] u = new float[3];
float norm;
norm = 1.0f / axis.Norm();
u[0] = axis.GetX() * norm;
u[1] = axis.GetY() * norm;
u[2] = axis.GetZ() * norm;
// add (cos(angle)-1)*(1 - u u').
float cos_angle = (float)Math.Cos(angle);
for (uint i = 0; i < 3; ++i)
{
for (uint j = 0; j < 3; ++j)
{
this[(int)i, (int)j] += (cos_angle - 1) * ((i == j ? 1:0) - u[i] * u[j]);
}
}
// add sin(angle) * [u]
float sin_angle = (float)Math.Sin(angle);
this[0, 1] -= sin_angle * u[2];
this[0, 2] += sin_angle * u[1];
this[1, 0] += sin_angle * u[2];
this[1, 2] -= sin_angle * u[0];
this[2, 0] -= sin_angle * u[1];
this[2, 1] += sin_angle * u[0];
}
public Vector3D InverseRotate(Vector3D v)
{
return
(new Vector3D(this[0, 0] * v.GetX() + this[1, 0] * v.GetY() + this[2, 0] * v.GetZ(),
this[0, 1] * v.GetX() + this[1, 1] * v.GetY() + this[2, 1] * v.GetZ(),
this[0, 2] * v.GetX() + this[1, 2] * v.GetY() + this[2, 2] * v.GetZ()));
}
//returns matrix of dimensions (3,4)
public static MatrixFixed dRq_times_a_by_dq (Quaternion q, Vector3D a)
{
MatrixFixed aMat = new MatrixFixed(3,1);
aMat[0, 0] = a.GetX();
aMat[1, 0] = a.GetY();
aMat[2, 0] = a.GetZ();
MatrixFixed TempR = new MatrixFixed(3,3);
MatrixFixed Temp31 = new MatrixFixed(3,1);
MatrixFixed dRq_times_a_by_dq = new MatrixFixed(3,4);
// Make Jacobian by stacking four vectors together side by side
TempR = dR_by_dq0(q);
Temp31 = TempR * aMat;
dRq_times_a_by_dq.Update(Temp31, 0, 0);
TempR = dR_by_dqx(q);
Temp31 = TempR * aMat;
dRq_times_a_by_dq.Update(Temp31, 0, 1);
TempR = dR_by_dqy(q);
Temp31 = TempR * aMat;
dRq_times_a_by_dq.Update(Temp31, 0, 2);
TempR = dR_by_dqz(q);
Temp31 = TempR * aMat;
dRq_times_a_by_dq.Update(Temp31, 0, 3);
return dRq_times_a_by_dq;
}
public static MatrixFixed dvnorm_by_dv(Vector3D v)
{
float vv = v.GetX()*v.GetX() + v.GetY()*v.GetY() + v.GetZ()*v.GetZ();
// We can use dqi_by_dqi and dqi_by_dqj functions because they are the same
float[] a = new float[9];
a[0] = dqi_by_dqi(v.GetX(), vv);
a[1] = dqi_by_dqj(v.GetX(), v.GetY(), vv);
a[2] = dqi_by_dqj(v.GetX(), v.GetZ(), vv);
a[3] = dqi_by_dqj(v.GetY(), v.GetX(), vv);
a[4] = dqi_by_dqi(v.GetY(), vv);
a[5] = dqi_by_dqj(v.GetY(), v.GetZ(), vv);
a[6] = dqi_by_dqj(v.GetZ(), v.GetX(), vv);
a[7] = dqi_by_dqj(v.GetZ(), v.GetY(), vv);
a[8] = dqi_by_dqi(v.GetZ(), vv);
MatrixFixed M = new MatrixFixed(3,3);
return M;
}
public float Multiply(LineSeg3D ls1, LineSeg3D ls2)
{
Vector3D d1 = new Vector3D(ls1._ep - ls1._sp);
Vector3D d2 = new Vector3D(ls2._ep - ls2._sp);
return (d1.GetX() * d2.GetX() + d1.GetY() * d2.GetY() + d1.GetZ() * d2.GetZ());
}
/// <summary>
/// turns a postion and quaternion based orientation into a matrix format
/// </summary>
/// <param name="position"></param>
/// <param name="orientation"></param>
/// <returns></returns>
private Matrix QuaternionToMatrixLookAt(Vector3D position, SceneLibrary.Quaternion orientation)
{
Microsoft.DirectX.Quaternion q_orientation =
new Microsoft.DirectX.Quaternion((float)orientation.GetX(),
(float)orientation.GetY(),
(float)orientation.GetZ(),
(float)orientation.GetR());
Matrix _matrixRotation = Matrix.RotationQuaternion(q_orientation);
Vector3 camPos = new Vector3((float)position.GetX(), -(float)position.GetY(), (float)position.GetZ());
Vector3 camTY = new Vector3(_matrixRotation.M21, _matrixRotation.M22, _matrixRotation.M23);
Vector3 camTZ = new Vector3(-(_matrixRotation.M31), -(_matrixRotation.M32), -(_matrixRotation.M33));
return (Matrix.LookAtLH(camPos, camPos + camTZ, camTY));
//float yaw = 0, pitch = 0, roll = 0;
//DecomposeRollPitchYawZXYMatrix(_matrixRotation, ref pitch, ref yaw, ref roll);
//return (Matrix.RotationYawPitchRoll((float)yaw, (float)pitch, (float)roll) * Matrix.LookAtLH(camPos, camPos + camTZ, camTY));
//return (Matrix.RotationYawPitchRoll(0, 0, (float)roll) * Matrix.LookAtLH(camPos, camPos + camTZ, camTY));
}
/// <summary>
/// Calculate commonly used Jacobian part \partial q(\omega * \Delta t) / \partial \omega .
/// </summary>
/// <param name="omega"></param>
/// <param name="delta_t"></param>
/// <param name="dqomegadt_by_domega"></param>
public void dqomegadt_by_domega(Vector3D omega, float delta_t, MatrixFixed dqomegadt_by_domega)
{
// Modulus
float omegamod = (float)Math.Sqrt(omega.GetX() * omega.GetX() +
omega.GetY() * omega.GetY() +
omega.GetZ() * omega.GetZ());
// Use generic ancillary functions to calculate components of Jacobian
dqomegadt_by_domega.Put(0, 0, dq0_by_domegaA(omega.GetX(), omegamod, delta_t));
dqomegadt_by_domega.Put(0, 1, dq0_by_domegaA(omega.GetY(), omegamod, delta_t));
dqomegadt_by_domega.Put(0, 2, dq0_by_domegaA(omega.GetZ(), omegamod, delta_t));
dqomegadt_by_domega.Put(1, 0, dqA_by_domegaA(omega.GetX(), omegamod, delta_t));
dqomegadt_by_domega.Put(1, 1, dqA_by_domegaB(omega.GetX(), omega.GetY(), omegamod, delta_t));
dqomegadt_by_domega.Put(1, 2, dqA_by_domegaB(omega.GetX(), omega.GetZ(), omegamod, delta_t));
dqomegadt_by_domega.Put(2, 0, dqA_by_domegaB(omega.GetY(), omega.GetX(), omegamod, delta_t));
dqomegadt_by_domega.Put(2, 1, dqA_by_domegaA(omega.GetY(), omegamod, delta_t));
dqomegadt_by_domega.Put(2, 2, dqA_by_domegaB(omega.GetY(), omega.GetZ(), omegamod, delta_t));
dqomegadt_by_domega.Put(3, 0, dqA_by_domegaB(omega.GetZ(), omega.GetX(), omegamod, delta_t));
dqomegadt_by_domega.Put(3, 1, dqA_by_domegaB(omega.GetZ(), omega.GetY(), omegamod, delta_t));
dqomegadt_by_domega.Put(3, 2, dqA_by_domegaA(omega.GetZ(), omegamod, delta_t));
}
public Vector3D InverseRotate(Vector3D v)
{
return
new Vector3D(this[0,0]*v.GetX()+this[1,0]*v.GetY()+this[2,0]*v.GetZ(),
this[0,1]*v.GetX()+this[1,1]*v.GetY()+this[2,1]*v.GetZ(),
this[0,2]*v.GetX()+this[1,2]*v.GetY()+this[2,2]*v.GetZ());
}
/// <summary>
/// Note that the axis doesn't have to be a unit vector.
/// </summary>
/// <param name="axis">the axis around which to rotate</param>
/// <param name="angle">the angle of rotation in radians</param>
public void Set(Vector3D axis, float angle)
{
// start with an identity matrix.
this.SetIdentity();
// Identity is all that can be deduced from zero angle
if (angle == 0)
return;
// normalize to a unit vector u
float[] u = new float[3];
float norm;
norm = 1.0f / axis.Norm();
u[0]=axis.GetX()*norm;
u[1]=axis.GetY()*norm;
u[2]=axis.GetZ()*norm;
// add (cos(angle)-1)*(1 - u u').
float cos_angle = (float)Math.Cos(angle);
for (uint i=0; i<3; ++i)
for (uint j=0; j<3; ++j)
this[(int)i,(int)j] += (cos_angle-1) * ((i==j ? 1:0) - u[i]*u[j]);
// add sin(angle) * [u]
float sin_angle = (float)Math.Sin(angle);
this[0,1] -= sin_angle*u[2];
this[0,2] += sin_angle*u[1];
this[1,0] += sin_angle*u[2];
this[1,2] -= sin_angle*u[0];
this[2,0] -= sin_angle*u[1];
this[2,1] += sin_angle*u[0];
}
public override void func_xpredef_and_dxpredef_by_dxp_and_dxpredef_by_dxpdef
(Vector xp, Vector xpdef)
{
// Split position state into cartesian and quaternion
// r0, q0: position in original frame
func_r(xp);
func_q(xp);
Vector3D r0 = rRES;
Quaternion q0 = qRES;
// rn, qn: definition of new frame
func_r(xpdef);
func_q(xpdef);
Vector3D rn = rRES;
Quaternion qn = qRES;
Quaternion qnbar = qn.Conjugate();
// Calculate new cartesian part of xp
Vector3D Temp31a = new Vector3D(r0 - rn); // Temp31a is r0 - rn
RotationMatrix Temp33a = qnbar.RotationMatrix();
Vector3D Temp31b = new Vector3D(Temp33a * Temp31a);
// Copy into xpredefRES
xpredefRES[0] = Temp31b.GetX();
xpredefRES[1] = Temp31b.GetY();
xpredefRES[2] = Temp31b.GetZ();
// Calculate new quaternion part of xp
Quaternion Tempqa = qnbar.Multiply(q0);
// Copy into xpredefRES
Vector vecTempqa = Tempqa.GetRXYZ();
xpredefRES.Update(vecTempqa, 3);
// Form Jacobian dxpredef_by_dxpRES
dxpredef_by_dxpRES.Fill(0.0f);
dxpredef_by_dxpRES.Update(Temp33a, 0, 0);
MatrixFixed Temp44a = MatrixFixed.dq3_by_dq1(qnbar);
dxpredef_by_dxpRES.Update(Temp44a, 3, 3);
// Form Jacobian dxpredef_by_dxpdefRES
dxpredef_by_dxpdefRES.Fill(0.0f);
//Temp33a *= -1.0f;
Temp33a = (RotationMatrix)(Temp33a * - 1.0f);
dxpredef_by_dxpdefRES.Update(Temp33a, 0, 0);
Temp44a = MatrixFixed.dq3_by_dq2(q0);
MatrixFixed Temp44b = MatrixFixed.dqbar_by_dq();
MatrixFixed Temp44c = Temp44a * Temp44b;
dxpredef_by_dxpdefRES.Update(Temp44c, 3, 3);
// Top right corner of this Jacobian is tricky because we have to
// differentiate a rotation matrix
// Uses function that does this in bits.cc
// Build this corner in Temp34a
//MatrixFixed temp = new MatrixFixed(3,4);
MatrixFixed Temp34a = MatrixFixed.dRq_times_a_by_dq(qnbar, Temp31a.GetVNL3());
// So far we have drN_by_dqnbar; want _by_dqn
MatrixFixed Temp34b = Temp34a * Temp44b;
// Finally copy into result matrix
dxpredef_by_dxpdefRES.Update(Temp34b, 0, 3);
// cout << "dxpredef_by_dxpdefRES" << dxpredef_by_dxpdefRES;
}
/// <summary>
///
/// </summary>
/// <param name="angleaxis">the 3D vector whose size gives the angle and direction</param>
public void Set(Vector3D angleaxis)
{
_x = angleaxis.GetX();
_y = angleaxis.GetY();
_z = angleaxis.GetZ();
}
/// <summary>
///
/// </summary>
/// <param name="axis">the axis (doesn't need to be a unit vector) around which to rotate</param>
/// <param name="angle">the rotation angle in radians</param>
public void Set(Vector3D axis, float angle)
{
_x = angle * axis.GetX();
_y = angle * axis.GetY();
_z = angle * axis.GetZ();
}
/// <summary>
///
/// </summary>
/// <param name="angleaxis">the 3D vector whose size gives the angle and direction</param>
public void Set(Vector3D angleaxis)
{
_x = angleaxis.GetX();
_y = angleaxis.GetY();
_z = angleaxis.GetZ();
}
/// <summary>
///
/// </summary>
/// <param name="axis">the axis (doesn't need to be a unit vector) around which to rotate</param>
/// <param name="angle">the rotation angle in radians</param>
public void Set(Vector3D axis, float angle)
{
_x = angle * axis.GetX();
_y = angle * axis.GetY();
_z = angle * axis.GetZ();
}