public Vector Mul( Vector v1 )
{
Vector r = new Vector();
for (int i = 0; i < 3; i++)
{
r.v[i] = 0.0f;
for (int j = 0; j < 3; j++)
{
r.v[i] += m[i, j] * v1.v[j];
}
}
return r;
}
void ComputeAngularVelocities()
{
Vector Axis;
Quaternion qrot = follow.Conjugate() * quat;
float angle = qrot.ToAngleAxis( out Axis );
Vector angularDisplacement = Axis * angle; // * Mathf.Deg2Rad;
//Vector angularSpeed = angularDisplacement / Time.deltaTime;
Velocities = angularDisplacement; // It's possible for this to accumulate extra spins, apparently - will need to be clamped -PI to +PI
follow = follow.Slerp( quat, 0.05f );
matFollow.From( follow );
}
void BuildCube()
{
CubePt[0] = new Vector( -cubeWidth, -cubeHeight, -cubeDepth );
CubePt[1] = new Vector( -cubeWidth, cubeHeight, -cubeDepth );
CubePt[2] = new Vector( cubeWidth, -cubeHeight, -cubeDepth );
CubePt[3] = new Vector( cubeWidth, cubeHeight, -cubeDepth );
CubePt[4] = new Vector( -cubeWidth, -cubeHeight, cubeDepth );
CubePt[5] = new Vector( -cubeWidth, cubeHeight, cubeDepth );
CubePt[6] = new Vector( cubeWidth, -cubeHeight, cubeDepth );
CubePt[7] = new Vector( cubeWidth, cubeHeight, cubeDepth );
CubePt[8] = new Vector( 0, 0, cubeDepth - 0.1f );
CubePt[9] = new Vector( 0, 0, cubeDepth + 0.1f );
Invalidate();
}
void BuildCube()
{
CubePt[0] = new Vector( -cubeWidth, -cubeHeight, -cubeDepth );
CubePt[1] = new Vector( -cubeWidth, cubeHeight, -cubeDepth );
CubePt[2] = new Vector( cubeWidth, -cubeHeight, -cubeDepth );
CubePt[3] = new Vector( cubeWidth, cubeHeight, -cubeDepth );
CubePt[4] = new Vector( -cubeWidth, -cubeHeight, cubeDepth );
CubePt[5] = new Vector( -cubeWidth, cubeHeight, cubeDepth );
CubePt[6] = new Vector( cubeWidth, -cubeHeight, cubeDepth );
CubePt[7] = new Vector( cubeWidth, cubeHeight, cubeDepth );
CubePt[8] = new Vector( 0, 0, cubeDepth - 0.1f );
CubePt[9] = new Vector( 0, 0, cubeDepth + 0.1f );
for(int i = 0; i < 16; i++)
{
QuadPt[i] = new Vector( Quad2d[i].X * cubeWidth, -cubeHeight * 0.1f, Quad2d[i].Y * cubeDepth );
QuadPt[i+16] = new Vector( Quad2d[i].X * cubeWidth, +cubeHeight * 0.1f, Quad2d[i].Y * cubeDepth );
}
QuadPt[32] = new Vector( 0.0f, 0.0f, 0.3f * cubeDepth );
QuadPt[33] = new Vector( 0.0f, 0.0f, 0.5f * cubeDepth );
Invalidate();
}
void DrawShape( Graphics g, Matrix m, Color col, Vector[] points, int[] lines )
{
for(int i = 0; i < points.Length; i++)
{
pt[i] = m.Mul( points[i] );
}
for(int i = 0; i < points.Length; i++)
{
pt[i].v[2] += ViewDist;
if(pt[i].v[2] == 0)
{
pt[i].v[2] = 0.0001f;
}
pt[i].v[0] /= pt[i].v[2];
pt[i].v[1] /= pt[i].v[2];
}
Pen penCol = new Pen( col );
PointF[] cb = new PointF[2];
for(int i = 0; i < lines.Length; i += 2)
{
cb[0].X = pt[lines[i]].v[0];
cb[0].Y = pt[lines[i]].v[1];
cb[1].X = pt[lines[i + 1]].v[0];
cb[1].Y = pt[lines[i + 1]].v[1];
cb[0].X = cb[0].X * DrawScale + CenterX;
cb[0].Y = cb[0].Y * -DrawScale + CenterY;
cb[1].X = cb[1].X * DrawScale + CenterX;
cb[1].Y = cb[1].Y * -DrawScale + CenterY;
g.DrawLine( penCol, cb[0], cb[1] );
}
}
// Returns an angle and an axis that represents the rotation of the quaternion
public float ToAngleAxis( out Vector Axis )
{
float tw = Math.Min( w, 1.0f );
float angle = 2.0f * (float)Math.Acos( tw );
Axis = new Vector();
float s = (float)Math.Sqrt( 1.0f - tw*tw ); // assuming quaternion normalised then w is less than 1, so term always positive.
s = Math.Max( s, 0.001f ); // prevent divide by zero
Axis.x = x / s; // normalise axis
Axis.y = y / s;
Axis.z = z / s;
return angle;
}
private float ComputeTiltCompensatedHeading()
{
// Compute pitch and roll from the current accelerometer vector - only accurate if stationary
Vector v = new Vector( AccelX, AccelY, AccelZ );
v = v.Normalize();
float accPitch = (float)Math.Asin( -v.x );
float accRoll = (float)Math.Asin( v.y / Math.Cos(accPitch) );
// Technically we should also calibrate the min/max readings from the mag first - this may not be accurate otherwise
float Mxh = (float)(MagX * Math.Cos( accPitch ) + MagZ * Math.Sin( accPitch ));
float Myh = (float)(MagX * Math.Sin( accRoll ) * Math.Sin( accPitch ) + MagY * Math.Cos( accRoll ) - MagZ * Math.Sin( accRoll ) * Math.Cos( accPitch ));
float Heading = (float)(Math.Atan2( Mxh, Myh ) * 1800.0 / Math.PI);
return Heading;
}
private void ComputeAltitudeEstimate()
{
acc = new Vector( -AccelX, AccelZ, AccelY );
acc *= 1.0f * AccScale;
// Get gravity vector from orentation matrix
Vector gravity = new Vector( m.m[1,0], m.m[1,1], m.m[1,2] );
// Subtract from accelerometer vector to get directional forces
acc -= gravity;
// acc is now m/s^2
acc *= 9.8f;
// Orient accelerometer vector (or at least just Z component)
Vector forceW = m.Transpose().Mul( acc );
// Compute the vertical velocity from the altimeter
altVelocity = ((float)(Alt - prevAlt) / 1000.0f) * UpdateRate; // Now in M/s
// Integrate accelerometer vector to get velocity (m/sec)
velocityEstimate += forceW * (1.0f / UpdateRate);
// Use the altimeter velocity estimate to drift correct the computed velocity
velocityEstimate.y = velocityEstimate.y * 0.998f + altVelocity * 0.002f;
// Integrate Z velocity to get approximate height (in meters)
positionEstimate += velocityEstimate * (1.0f / UpdateRate);
// Slowly un-drift the Y position estimate with the altimeter over time
positionEstimate.y = (positionEstimate.y * 0.998f) + (((float)Alt / 1000.0f) * 0.002f);
lblStatOutput.Text = string.Format( "{0:0.00} {1:0.000}", positionEstimate.y, velocityEstimate.y );
}
void ComputeQuatAlternateMethod()
{
// Trapezoidal integration of gyro readings
float rx = (float)((GyroX+lastGx)*0.5f - GZeroX) * GyroScale + errCorrX2;
float ry = (float)((GyroZ+lastGz)*0.5f - GZeroZ) * -GyroScale + errCorrY2;
float rz = (float)((GyroY+lastGy)*0.5f - GZeroY) * -GyroScale + errCorrZ2;
lastGx = GyroX;
lastGy = GyroY;
lastGz = GyroZ;
float rMag = (float)(Math.Sqrt(rx * rx + ry * ry + rz * rz + 0.0000000001) * 0.5);
float cosr = (float)Math.Cos(rMag);
float sinr = (float)Math.Sin(rMag) / rMag;
qdot.w = -(rx * Q2.x + ry * Q2.y + rz * Q2.z) * 0.5f;
qdot.x = (rx * Q2.w + rz * Q2.y - ry * Q2.z) * 0.5f;
qdot.y = (ry * Q2.w - rz * Q2.x + rx * Q2.z) * 0.5f;
qdot.z = (rz * Q2.w + ry * Q2.x - rx * Q2.y) * 0.5f;
Q2.w = cosr * Q2.w + sinr * qdot.w;
Q2.x = cosr * Q2.x + sinr * qdot.x;
Q2.y = cosr * Q2.y + sinr * qdot.y;
Q2.z = cosr * Q2.z + sinr * qdot.z;
Q2 = Q2.Normalize();
// Convert to matrix form
m2.From(Q2);
// Compute the difference between the accelerometer vector and the matrix Y (up) vector
acc = new Vector( -AccelX, AccelZ, AccelY );
rMag = acc.Length * AccScale;
acc = acc.Normalize();
float accWeight = 1.0f - Math.Min( Math.Abs( 2.0f - rMag * 2.0f ), 1.0f );
// accWeight *= accWeight * 4.0f;
float errDiffX = acc.y * m2.m[1,2] - acc.z * m2.m[1,1];
float errDiffY = acc.z * m2.m[1,0] - acc.x * m2.m[1,2];
float errDiffZ = acc.x * m2.m[1,1] - acc.y * m2.m[1,0];
accWeight *= 1.0f / 512.0f;
errCorrX2 = errDiffX * accWeight;
errCorrY2 = errDiffY * accWeight;
errCorrZ2 = errDiffZ * accWeight;
// At this point, errCorr represents a very small correction rotation vector, but in the WORLD frame
// Rotate it into the current BODY frame
//Vector errVect = new Vector( errCorrX2, errCorrY2, errCorrZ2 );
//errVect = m.Transpose().Mul( errVect );
//errCorrX2 = errVect.x;
//errCorrY2 = errVect.y;
//errCorrZ2 = errVect.z;
}
void ComputeQuatOriginalMethod()
{
float rx = (float)(GyroX - GZeroX) * GyroScale + errCorrX1;
float ry = (float)(GyroZ - GZeroZ) * -GyroScale + errCorrY1;
float rz = (float)(GyroY - GZeroY) * -GyroScale + errCorrZ1;
float rMag = (float)(Math.Sqrt(rx * rx + ry * ry + rz * rz + 0.0000000001) * 0.5);
float cosr = (float)Math.Cos(rMag);
float sinr = (float)Math.Sin(rMag) / rMag;
qdot.w = -(rx * Q1.x + ry * Q1.y + rz * Q1.z) * 0.5f;
qdot.x = (rx * Q1.w + rz * Q1.y - ry * Q1.z) * 0.5f;
qdot.y = (ry * Q1.w - rz * Q1.x + rx * Q1.z) * 0.5f;
qdot.z = (rz * Q1.w + ry * Q1.x - rx * Q1.y) * 0.5f;
Q1.w = cosr * Q1.w + sinr * qdot.w;
Q1.x = cosr * Q1.x + sinr * qdot.x;
Q1.y = cosr * Q1.y + sinr * qdot.y;
Q1.z = cosr * Q1.z + sinr * qdot.z;
Q1 = Q1.Normalize();
// Convert to matrix form
m.From(Q1);
// Compute the difference between the accelerometer vector and the matrix Y (up) vector
acc = new Vector( -AccelX, AccelZ, AccelY );
rMag = acc.Length * AccScale;
acc = acc.Normalize();
float accWeight = 1.0f - Math.Min( Math.Abs( 2.0f - rMag * 2.0f ), 1.0f );
float errDiffX = acc.y * m.m[1,2] - acc.z * m.m[1,1];
float errDiffY = acc.z * m.m[1,0] - acc.x * m.m[1,2];
float errDiffZ = acc.x * m.m[1,1] - acc.y * m.m[1,0];
accWeight *= 1.0f / 512.0f;
errCorrX1 = errDiffX * accWeight;
errCorrY1 = errDiffY * accWeight;
errCorrZ1 = errDiffZ * accWeight;
}
