Vector3f normalizeVector3f(const Vector3f &v, const float maxLength )
{
if ( v.Length() > maxLength )
return v.Normalized() * maxLength;
else
return v;
}
void Matrix::LookAt(const Vector3f & inEye, const Vector3f & inDirection, const Vector3f & inUp)
{
Vector3f forward = inDirection.Normalized();
Vector3f up = inUp.Normalized();
Vector3f right = forward.Cross(up);
right.Normalize();
up = right.Cross(forward);
// Make inverse rotation matrix using right, forward, up vectors
Set( right.x(), right.y(), right.z(), 0.0f,
up.x(), up.y(), up.z(), 0.0f,
forward.x(), forward.y(), forward.z(), 0.0f,
0.0f, 0.0f, 0.0f, 1.0f);
Pos() = Rotate(-inEye);
}
void BoundingSphere::Encapsulate(const Vector3f &v)
{
// TODO : check if this is correct
Vector3f diff = v - center;
float dist = diff.Dot(diff);
if (dist > radiusSq)
{
Vector3f diff2 = diff.Normalized() * radius;
Vector3f delta = 0.5f * (diff - diff2);
center += delta;
radius += delta.Length();
radiusSq = radius*radius;
}
}
Quaternion Transform::GetLookAtDirection(Vector3f point, Vector3f up)
{
Vector3f lookDir = (point - pos).Normalized();
up = up.Normalized();
// .. What if up and lookDir are parallel?
if (up == lookDir || up == -lookDir)
{
lookDir.y += 0.00000001f;
lookDir.x += 0.00000001f;
}
Vector3f right = up.Cross(lookDir).Normalized();
Vector3f pUp = lookDir.Cross(right).Normalized();
return Quaternion(Matrix4f().InitRotation(lookDir, pUp));
//return Quaternion(Matrix4f().InitRotation(lookDir, up));
}
void SensorFusion::handleMessage(const MessageBodyFrame& msg)
{
if (msg.Type != Message_BodyFrame || !IsMotionTrackingEnabled())
return;
// Put the sensor readings into convenient local variables
Vector3f gyro = msg.RotationRate;
Vector3f accel = msg.Acceleration;
Vector3f mag = msg.MagneticField;
// Insert current sensor data into filter history
FRawMag.AddElement(mag);
FAngV.AddElement(gyro);
// Apply the calibration parameters to raw mag
Vector3f calMag = MagCalibrated ? GetCalibratedMagValue(FRawMag.Mean()) : FRawMag.Mean();
// Set variables accessible through the class API
DeltaT = msg.TimeDelta;
AngV = gyro;
A = accel;
RawMag = mag;
CalMag = calMag;
// Keep track of time
Stage++;
RunningTime += DeltaT;
// Small preprocessing
Quatf Qinv = Q.Inverted();
Vector3f up = Qinv.Rotate(Vector3f(0, 1, 0));
Vector3f gyroCorrected = gyro;
// Apply integral term
// All the corrections are stored in the Simultaneous Orthogonal Rotations Angle representation,
// which allows to combine and scale them by just addition and multiplication
if (EnableGravity || EnableYawCorrection)
gyroCorrected -= GyroOffset;
if (EnableGravity)
{
const float spikeThreshold = 0.01f;
const float gravityThreshold = 0.1f;
float proportionalGain = 5 * Gain; // Gain parameter should be removed in a future release
float integralGain = 0.0125f;
Vector3f tiltCorrection = SensorFusion_ComputeCorrection(accel, up);
if (Stage > 5)
{
// Spike detection
float tiltAngle = up.Angle(accel);
TiltAngleFilter.AddElement(tiltAngle);
if (tiltAngle > TiltAngleFilter.Mean() + spikeThreshold)
proportionalGain = integralGain = 0;
// Acceleration detection
const float gravity = 9.8f;
if (fabs(accel.Length() / gravity - 1) > gravityThreshold)
integralGain = 0;
}
else // Apply full correction at the startup
{
proportionalGain = 1 / DeltaT;
integralGain = 0;
}
gyroCorrected += (tiltCorrection * proportionalGain);
GyroOffset -= (tiltCorrection * integralGain * DeltaT);
}
if (EnableYawCorrection && MagCalibrated && RunningTime > 2.0f)
{
const float maxMagRefDist = 0.1f;
const float maxTiltError = 0.05f;
float proportionalGain = 0.01f;
float integralGain = 0.0005f;
// Update the reference point if needed
if (MagRefIdx < 0 || calMag.Distance(MagRefsInBodyFrame[MagRefIdx]) > maxMagRefDist)
{
// Delete a bad point
if (MagRefIdx >= 0 && MagRefScore < 0)
{
MagNumReferences--;
MagRefsInBodyFrame[MagRefIdx] = MagRefsInBodyFrame[MagNumReferences];
MagRefsInWorldFrame[MagRefIdx] = MagRefsInWorldFrame[MagNumReferences];
}
// Find a new one
MagRefIdx = -1;
MagRefScore = 1000;
float bestDist = maxMagRefDist;
for (int i = 0; i < MagNumReferences; i++)
{
float dist = calMag.Distance(MagRefsInBodyFrame[i]);
if (bestDist > dist)
{
bestDist = dist;
MagRefIdx = i;
}
}
// Create one if needed
if (MagRefIdx < 0 && MagNumReferences < MagMaxReferences)
{
MagRefIdx = MagNumReferences;
MagRefsInBodyFrame[MagRefIdx] = calMag;
MagRefsInWorldFrame[MagRefIdx] = Q.Rotate(calMag).Normalized();
MagNumReferences++;
}
}
if (MagRefIdx >= 0)
{
Vector3f magEstimated = Qinv.Rotate(MagRefsInWorldFrame[MagRefIdx]);
Vector3f magMeasured = calMag.Normalized();
// Correction is computed in the horizontal plane (in the world frame)
Vector3f yawCorrection = SensorFusion_ComputeCorrection(magMeasured.ProjectToPlane(up),
magEstimated.ProjectToPlane(up));
if (fabs(up.Dot(magEstimated - magMeasured)) < maxTiltError)
{
MagRefScore += 2;
}
else // If the vertical angle is wrong, decrease the score
{
MagRefScore -= 1;
proportionalGain = integralGain = 0;
}
gyroCorrected += (yawCorrection * proportionalGain);
GyroOffset -= (yawCorrection * integralGain * DeltaT);
}
}
// Update the orientation quaternion based on the corrected angular velocity vector
Q = Q * Quatf(gyroCorrected, gyroCorrected.Length() * DeltaT);
// The quaternion magnitude may slowly drift due to numerical error,
// so it is periodically normalized.
if (Stage % 500 == 0)
Q.Normalize();
}