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