Example #1
0
Vector3f normalizeVector3f(const Vector3f &v, const float maxLength )
{
	if ( v.Length() > maxLength )
		return v.Normalized() * maxLength;
	else
		return v;
}
Example #2
0
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);
}
Example #3
0
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));
}
Example #5
0
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();
}