public void PushTransform(CarTimestamp timestamp, Matrix4 transform) { lock (lockobj) { TransformEntry te = new TransformEntry(timestamp, transform); rollingQueue.Add(te); // check if this is the most recent timestamp if (!double.IsNaN(recentTimestamp.ts) && timestamp < recentTimestamp) { // do a reverse bubble-sort int i = rollingQueue.Count-2; // find the insertion point while (i >= 0 && rollingQueue[i].timestamp > timestamp) { // shift the entry up rollingQueue[i+1] = rollingQueue[i]; i--; } // i+1 contains the empty slot to insert at rollingQueue[i+1] = te; } else { // update the recent timestamp recentTimestamp = timestamp; } if (!rollingQueue.VerifySort(delegate(TransformEntry l, TransformEntry r) { return l.timestamp.CompareTo(r.timestamp); })) { Trace.TraceError("relative transform sort is donzoed, flushing queue"); Reset(); } } }
public TransformEntry(CarTimestamp timestamp, Matrix4 transform) { this.timestamp = timestamp; this.transform = transform; }
public static Matrix4 FromSubmatrix( Matrix2 A, Matrix2 B, Matrix2 C, Matrix2 D) { Matrix4 ret = new Matrix4(); for (int i = 0; i < 2; i++) { for (int j = 0; j < 2; j++) { ret[i, j] = A[i, j]; ret[i+2, j] = C[i, j]; ret[i, j+2] = B[i, j]; ret[i+2, j+2] = D[i, j]; } } return ret; }
public static Matrix4 YPR(double yaw, double pitch, double roll) { double cy = Math.Cos(yaw), sy = Math.Sin(yaw); double cp = Math.Cos(pitch), sp = Math.Sin(pitch); double cr = Math.Cos(roll), sr = Math.Sin(roll); Matrix4 rz = new Matrix4( cy, -sy, 0, 0, sy, cy, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 ); Matrix4 ry = new Matrix4( cp, 0, sp, 0, 0, 1, 0, 0, -sp, 0, cp, 0, 0, 0, 0, 1 ); Matrix4 rx = new Matrix4( 1, 0, 0, 0, 0, cr, -sr, 0, 0, sr, cr, 0, 0, 0, 0, 1 ); return rx*ry*rz; }
public static Matrix4 operator *(Matrix4 l, Matrix4 r) { // [ ld0*rd0+ld1*rd4+ld2*rd8+ld3*rd12, ld0*rd1+ld1*rd5+ld2*rd9+ld3*rd13, ld0*rd2+ld1*rd6+ld2*rd10+ld3*rd14, ld0*rd3+ld1*rd7+ld2*rd11+ld3*rd15] // [ ld4*rd0+ld5*rd4+ld6*rd8+ld7*rd12, ld4*rd1+ld5*rd5+ld6*rd9+ld7*rd13, ld4*rd2+ld5*rd6+ld6*rd10+ld7*rd14, ld4*rd3+ld5*rd7+ld6*rd11+ld7*rd15] // [ ld8*rd0+ld9*rd4+ld10*rd8+ld11*rd12, ld8*rd1+ld9*rd5+ld10*rd9+ld11*rd13, ld8*rd2+ld9*rd6+ld10*rd10+ld11*rd14, ld8*rd3+ld9*rd7+ld10*rd11+ld11*rd15] // [ ld12*rd0+ld13*rd4+ld14*rd8+ld15*rd12, ld12*rd1+ld13*rd5+ld14*rd9+ld15*rd13, ld12*rd2+ld13*rd6+ld14*rd10+ld15*rd14, ld12*rd3+ld13*rd7+ld14*rd11+ld15*rd15] Matrix4 ret = new Matrix4(); for (int i = 0; i < 4; i++) { for (int j = 0; j < 4; j++) { ret[i, j] = l[i,0]*r[0,j] + l[i,1]*r[1,j] + l[i,2]*r[2,j] + l[i,3]*r[3,j]; } } return ret; }