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); } }