void vicon_DataReceived(object sender, IViconFrameEventArgs e)
{
if (e.Frame != null && e.Frame.Capacity > 0)
{
double ax, ay, az, magnitude, tempX, tempY, tempZ, cosine, sine, temp;
Matrix3 matrix = new Matrix3();
pose.x = (e.Frame.ElementAt(0))/1000;
pose.y = (e.Frame.ElementAt(1))/1000;
pose.z = (e.Frame.ElementAt(2))/1000;
pose.timestamp = TimeSync.CurrentTime;
// Changing from axis-angle that Vicon gives to rotation matrix
ax = e.Frame.ElementAt(3);
ay = e.Frame.ElementAt(4);
az = e.Frame.ElementAt(5);
magnitude = Math.Sqrt(ax * ax + ay * ay + az * az);
tempX = ax / magnitude;
tempY = ay / magnitude;
tempZ = az / magnitude;
cosine = Math.Cos(magnitude);
sine = Math.Sin(magnitude);
temp = 1 - cosine;
matrix[0, 0] = cosine + temp * tempX * tempX;
matrix[0, 1] = temp * tempX * tempY + sine * tempZ;
matrix[0, 2] = temp * tempX * tempZ - sine * tempY;
matrix[1, 0] = temp * tempY * tempX - sine * tempZ;
matrix[1, 1] = cosine + temp * tempY * tempY;
matrix[1, 2] = temp * tempY * tempZ + sine * tempX;
matrix[2, 0] = temp * tempZ * tempX + sine * tempY;
matrix[2, 1] = temp * tempZ * tempY - sine * tempX;
matrix[2, 2] = cosine + temp * tempZ * tempZ;
// Changing from rotation matrix to pitch, yaw, roll
pose.pitch = matrix.ExtractYPR().Y;
pose.yaw = -matrix.ExtractYPR().X;
pose.roll = -matrix.ExtractYPR().Z;
if (NewPoseAvailable != null)
NewPoseAvailable(this, new NewPoseAvailableEventArgs(pose));
}
}
public void Transform(Matrix3 m)
{
Vector3 s = new Vector3(start.X, start.Y, 1);
Vector3 e = new Vector3(end.X, end.Y, 1);
s = m * s;
e = m * e;
start = new Vector2(s.X, s.Y);
end = new Vector2(e.X, e.Y);
}
public void Transform(Matrix3 m)
{
Vector3 p0 = new Vector3(cb.P0.X, cb.P0.Y, 1);
Vector3 p1 = new Vector3(cb.P1.X, cb.P1.Y, 1);
Vector3 p2 = new Vector3(cb.P2.X, cb.P2.Y, 1);
Vector3 p3 = new Vector3(cb.P3.X, cb.P3.Y, 1);
p0 = m * p0;
p1 = m * p1;
p2 = m * p2;
p3 = m * p3;
cb = new CubicBezier(new Vector2(p0.X, p0.Y), new Vector2(p1.X, p1.Y),
new Vector2(p2.X, p2.Y), new Vector2(p3.X, p3.Y));
}
public Matrix3 ToDCM()
{
Matrix3 dcm = new Matrix3();
//Assign the values to the dcm
for (int i = 1; i < 3; i++)
{
for (int j = 0; j < 3; j++)
dcm[i, j] = this[i, j];
}
return dcm;
}
public static Matrix3 DCM(double ez, double ey, double ex)
{
//Initialize the matrix as empty zeros
Matrix3 T = new Matrix3();
//Create the direction cosine matrix (zyx - euler rotations)
//Compute intermediate terms
double cz = Math.Cos(ez);
double sz = Math.Sin(ez);
double cy = Math.Cos(ey);
double sy = Math.Sin(ey);
double cx = Math.Cos(ex);
double sx = Math.Sin(ex);
/* From Matlab angle2dcm (zyx)
[ cy*cz, cy*sz, -sy]
[ sy*sx*cz-sz*cx, sy*sx*sz+cz*cx, cy*sx]
[ sy*cx*cz+sz*sx, sy*cx*sz-cz*sx, cy*cx]*/
T[0, 0] = cy * cz;
T[0, 1] = cy * sz;
T[0, 2] = -sy;
T[1, 0] = sy * sx * cz - sz * cx;
T[1, 1] = sy * sx * sz + cz * cx;
T[1, 2] = cy * sx;
T[2, 0] = sy * cx * cz + sz * sx;
T[2, 1] = sy * cx * sz - cz * sx;
T[2, 2] = cy * cx;
return T;
}
public void Transform(Matrix3 m)
{
foreach (IPathSegment seg in segments)
{
seg.Transform(m);
}
}
