SDK 0.2.2

1 files changed, 180 insertions, 0 deletions

diff --git a/LibOVR/Src/Util/Util_MagCalibration.cpp b/LibOVR/Src/Util/Util_MagCalibration.cpp
new file mode 100644
index 0000000..f3e72f5
--- /dev/null
+++ b/LibOVR/Src/Util/Util_MagCalibration.cpp
@@ -0,0 +1,180 @@
+/************************************************************************************

+

+Filename    :   Util_MagCalibration.cpp

+Content     :   Procedures for calibrating the magnetometer

+Created     :   April 16, 2013

+Authors     :   Steve LaValle, Andrew Reisse

+

+Copyright   :   Copyright 2013 Oculus VR, Inc. All Rights reserved.

+

+Use of this software is subject to the terms of the Oculus license

+agreement provided at the time of installation or download, or which

+otherwise accompanies this software in either electronic or hard copy form.

+

+*************************************************************************************/

+

+#include "Util_MagCalibration.h"

+

+namespace OVR { namespace Util {

+

+void MagCalibration::BeginAutoCalibration(SensorFusion& sf)

+{

+    Status = Mag_AutoCalibrating;

+    // This is a "hard" reset of the mag, so need to clear stored values

+    sf.ClearMagCalibration();

+    SampleCount = 0;

+}

+

+unsigned MagCalibration::UpdateAutoCalibration(SensorFusion& sf)

+{

+    if (Status != Mag_AutoCalibrating)

+        return Status;

+

+    Quatf q = sf.GetOrientation();

+    Vector3f m = sf.GetMagnetometer();

+

+    InsertIfAcceptable(q, m);

+

+    if ((SampleCount == 4) && (Status == Mag_AutoCalibrating))

+        SetCalibration(sf);

+

+    return Status;

+

+}

+

+void MagCalibration::BeginManualCalibration(SensorFusion& sf)

+{

+    Status = Mag_ManuallyCalibrating;

+    sf.ClearMagCalibration();

+    SampleCount = 0;

+}

+

+bool MagCalibration::IsAcceptableSample(const Quatf& q, const Vector3f& m)

+{

+    switch (SampleCount)

+    {

+        // Initial sample is always acceptable

+    case 0:

+        return true;

+        break;

+    case 1:

+        return (q.DistanceSq(QuatSamples[0]) > MinQuatDistanceSq)&&

+               ((m - MagSamples[0]).LengthSq() > MinMagDistanceSq);

+        break;

+    case 2:

+        return (q.DistanceSq(QuatSamples[0]) > MinQuatDistanceSq)&&

+               (q.DistanceSq(QuatSamples[1]) > MinQuatDistanceSq)&&

+               ((m - MagSamples[0]).LengthSq() > MinMagDistanceSq)&&

+               ((m - MagSamples[1]).LengthSq() > MinMagDistanceSq);

+        break;

+    case 3:

+        return (q.DistanceSq(QuatSamples[0]) > MinQuatDistanceSq)&&

+               (q.DistanceSq(QuatSamples[1]) > MinQuatDistanceSq)&&

+               (q.DistanceSq(QuatSamples[2]) > MinQuatDistanceSq)&&

+               ((PointToPlaneDistance(MagSamples[0],MagSamples[1],MagSamples[2],m) > MinMagDistance)||

+                (PointToPlaneDistance(MagSamples[1],MagSamples[2],m,MagSamples[0]) > MinMagDistance)||

+                (PointToPlaneDistance(MagSamples[2],m,MagSamples[0],MagSamples[1]) > MinMagDistance)||

+                (PointToPlaneDistance(m,MagSamples[0],MagSamples[1],MagSamples[2]) > MinMagDistance));

+    }

+

+    return false;

+}

+

+

+bool MagCalibration::InsertIfAcceptable(const Quatf& q, const Vector3f& m)

+{

+    if (IsAcceptableSample(q, m))

+    {

+        MagSamples[SampleCount] = m;

+        QuatSamples[SampleCount] = q;

+        SampleCount++;

+        return true;

+    }

+

+    return false;

+}

+

+

+bool MagCalibration::SetCalibration(SensorFusion& sf)

+{

+    if (SampleCount < 4)

+        return false;

+

+    MagCenter = CalculateSphereCenter(MagSamples[0],MagSamples[1],MagSamples[2],MagSamples[3]);

+    Matrix4f calMat = Matrix4f();

+    calMat.M[0][3] = -MagCenter.x;

+    calMat.M[1][3] = -MagCenter.y;

+    calMat.M[2][3] = -MagCenter.z;

+    sf.SetMagCalibration(calMat);

+    Status = Mag_Calibrated;

+    //LogText("MagCenter: %f %f %f\n",MagCenter.x,MagCenter.y,MagCenter.z);

+

+    return true;

+}

+

+

+// Calculate the center of a sphere that passes through p1, p2, p3, p4

+Vector3f MagCalibration::CalculateSphereCenter(const Vector3f& p1, const Vector3f& p2,

+                                               const Vector3f& p3, const Vector3f& p4) 

+{

+    Matrix4f A;

+    int i;

+    Vector3f p[4];

+    p[0] = p1;

+    p[1] = p2;

+    p[2] = p3;

+    p[3] = p4;

+

+    for (i = 0; i < 4; i++) 

+    {

+        A.M[i][0] = p[i].x;

+        A.M[i][1] = p[i].y;

+        A.M[i][2] = p[i].z;

+        A.M[i][3] = 1.0f;

+    }

+    float m11 = A.Determinant();

+    OVR_ASSERT(m11 != 0.0f);

+

+    for (i = 0; i < 4; i++) 

+    {

+        A.M[i][0] = p[i].x*p[i].x + p[i].y*p[i].y + p[i].z*p[i].z;

+        A.M[i][1] = p[i].y;

+        A.M[i][2] = p[i].z;

+        A.M[i][3] = 1.0f;

+    }

+    float m12 = A.Determinant();

+

+    for (i = 0; i < 4; i++) 

+    {

+        A.M[i][0] = p[i].x*p[i].x + p[i].y*p[i].y + p[i].z*p[i].z;

+        A.M[i][1] = p[i].x;

+        A.M[i][2] = p[i].z;

+        A.M[i][3] = 1.0f;

+    }

+    float m13 = A.Determinant();

+

+    for (i = 0; i < 4; i++) 

+    {

+        A.M[i][0] = p[i].x*p[i].x + p[i].y*p[i].y + p[i].z*p[i].z;

+        A.M[i][1] = p[i].x;

+        A.M[i][2] = p[i].y;

+        A.M[i][3] = 1.0f;

+    }

+    float m14 = A.Determinant();

+

+    float c = 0.5f / m11;

+    return Vector3f(c*m12, -c*m13, c*m14);

+}

+

+// Distance from p4 to the nearest point on a plane through p1, p2, p3

+float MagCalibration::PointToPlaneDistance(const Vector3f& p1, const Vector3f& p2,

+                                           const Vector3f& p3, const Vector3f& p4) 

+{

+    Vector3f v1 = p1 - p2;

+    Vector3f v2 = p1 - p3;

+    Vector3f planeNormal = v1.Cross(v2);

+    planeNormal.Normalize();

+    return (fabs((planeNormal * p4) - planeNormal * p1));

+}

+

+}}