Add OculusSDK 0.3.2 Linux Source Code w/o Samples, docs or binaries (libs or tools)

1 files changed, 245 insertions, 0 deletions

diff --git a/LibOVR/Src/OVR_SensorImpl_Common.cpp b/LibOVR/Src/OVR_SensorImpl_Common.cpp
new file mode 100644
index 0000000..99febe8
--- /dev/null
+++ b/LibOVR/Src/OVR_SensorImpl_Common.cpp
@@ -0,0 +1,245 @@
+/************************************************************************************
+
+Filename    :   OVR_SensorImpl_Common.cpp
+Content     :   Source common to SensorImpl and Sensor2Impl.
+Created     :   January 21, 2014
+Authors     :   Lee Cooper
+
+Copyright   :   Copyright 2014 Oculus VR, Inc. All Rights reserved.
+
+Licensed under the Oculus VR Rift SDK License Version 3.1 (the "License"); 
+you may not use the Oculus VR Rift SDK except in compliance with the License, 
+which is provided at the time of installation or download, or which 
+otherwise accompanies this software in either electronic or hard copy form.
+
+You may obtain a copy of the License at
+
+http://www.oculusvr.com/licenses/LICENSE-3.1 
+
+Unless required by applicable law or agreed to in writing, the Oculus VR SDK 
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+
+*************************************************************************************/
+
+#include "OVR_SensorImpl_Common.h"
+#include "Kernel/OVR_Alg.h"
+
+namespace OVR 
+{
+
+void UnpackSensor(const UByte* buffer, SInt32* x, SInt32* y, SInt32* z)
+{
+    // Sign extending trick
+    // from http://graphics.stanford.edu/~seander/bithacks.html#FixedSignExtend
+    struct {SInt32 x:21;} s;
+
+    *x = s.x = (buffer[0] << 13) | (buffer[1] << 5) | ((buffer[2] & 0xF8) >> 3);
+    *y = s.x = ((buffer[2] & 0x07) << 18) | (buffer[3] << 10) | (buffer[4] << 2) |
+               ((buffer[5] & 0xC0) >> 6);
+    *z = s.x = ((buffer[5] & 0x3F) << 15) | (buffer[6] << 7) | (buffer[7] >> 1);
+}
+
+void PackSensor(UByte* buffer, SInt32 x, SInt32 y, SInt32 z)
+{
+    // Pack 3 32 bit integers into 8 bytes
+    buffer[0] = UByte(x >> 13);
+    buffer[1] = UByte(x >> 5);
+    buffer[2] = UByte((x << 3) | ((y >> 18) & 0x07));
+    buffer[3] = UByte(y >> 10);
+    buffer[4] = UByte(y >> 2);
+    buffer[5] = UByte((y << 6) | ((z >> 15) & 0x3F));
+    buffer[6] = UByte(z >> 7);
+    buffer[7] = UByte(z << 1);
+}
+
+UInt16 SelectSensorRampValue(const UInt16* ramp, unsigned count,
+                                    float val, float factor, const char* label)
+{    
+    UInt16 threshold = (UInt16)(val * factor);
+
+    for (unsigned i = 0; i<count; i++)
+    {
+        if (ramp[i] >= threshold)
+            return ramp[i];
+    }
+    OVR_DEBUG_LOG(("SensorDevice::SetRange - %s clamped to %0.4f",
+                   label, float(ramp[count-1]) / factor));
+    OVR_UNUSED2(factor, label);
+    return ramp[count-1];
+}
+
+SensorRangeImpl::SensorRangeImpl(const SensorRange& r, UInt16 commandId)
+{
+    SetSensorRange(r, commandId);
+}
+
+void SensorRangeImpl::SetSensorRange(const SensorRange& r, UInt16 commandId)
+{
+    CommandId  = commandId;
+    AccelScale = SelectSensorRampValue(AccelRangeRamp, sizeof(AccelRangeRamp)/sizeof(AccelRangeRamp[0]),
+                                        r.MaxAcceleration, (1.0f / 9.81f), "MaxAcceleration");
+    GyroScale  = SelectSensorRampValue(GyroRangeRamp, sizeof(GyroRangeRamp)/sizeof(GyroRangeRamp[0]),
+                                        r.MaxRotationRate, Math<float>::RadToDegreeFactor, "MaxRotationRate");
+    MagScale   = SelectSensorRampValue(MagRangeRamp, sizeof(MagRangeRamp)/sizeof(MagRangeRamp[0]),
+                                        r.MaxMagneticField, 1000.0f, "MaxMagneticField");
+    Pack();
+}
+
+void SensorRangeImpl::GetSensorRange(SensorRange* r)
+{
+    r->MaxAcceleration = AccelScale * 9.81f;
+    r->MaxRotationRate = DegreeToRad((float)GyroScale);
+    r->MaxMagneticField= MagScale * 0.001f;
+}
+
+SensorRange SensorRangeImpl::GetMaxSensorRange()
+{
+    return SensorRange(AccelRangeRamp[sizeof(AccelRangeRamp)/sizeof(AccelRangeRamp[0]) - 1] * 9.81f,
+                        GyroRangeRamp[sizeof(GyroRangeRamp)/sizeof(GyroRangeRamp[0]) - 1] *
+                            Math<float>::DegreeToRadFactor,
+                        MagRangeRamp[sizeof(MagRangeRamp)/sizeof(MagRangeRamp[0]) - 1] * 0.001f);
+}
+
+void SensorRangeImpl::Pack()
+{
+    Buffer[0] = 4;
+    Buffer[1] = UByte(CommandId & 0xFF);
+    Buffer[2] = UByte(CommandId >> 8);
+    Buffer[3] = UByte(AccelScale);
+    Buffer[4] = UByte(GyroScale & 0xFF);
+    Buffer[5] = UByte(GyroScale >> 8);
+    Buffer[6] = UByte(MagScale & 0xFF);
+    Buffer[7] = UByte(MagScale >> 8);
+}
+
+void SensorRangeImpl::Unpack()
+{
+    CommandId = Buffer[1] | (UInt16(Buffer[2]) << 8);
+    AccelScale= Buffer[3];
+    GyroScale = Buffer[4] | (UInt16(Buffer[5]) << 8);
+    MagScale  = Buffer[6] | (UInt16(Buffer[7]) << 8);
+}
+
+SensorConfigImpl::SensorConfigImpl() 
+    :   CommandId(0), Flags(0), PacketInterval(0), SampleRate(0)
+{
+    memset(Buffer, 0, PacketSize);
+    Buffer[0] = 2;
+}
+
+void SensorConfigImpl::SetSensorCoordinates(bool sensorCoordinates)
+{ 
+    Flags = (Flags & ~Flag_SensorCoordinates) | (sensorCoordinates ? Flag_SensorCoordinates : 0); 
+}
+
+bool SensorConfigImpl::IsUsingSensorCoordinates() const
+{ 
+    return (Flags & Flag_SensorCoordinates) != 0; 
+}
+
+void SensorConfigImpl::Pack()
+{
+    Buffer[0] = 2;
+    Buffer[1] = UByte(CommandId & 0xFF);
+    Buffer[2] = UByte(CommandId >> 8);
+    Buffer[3] = Flags;
+    Buffer[4] = UByte(PacketInterval);
+    Buffer[5] = UByte(SampleRate & 0xFF);
+    Buffer[6] = UByte(SampleRate >> 8);
+}
+
+void SensorConfigImpl::Unpack()
+{
+    CommandId		= Buffer[1] | (UInt16(Buffer[2]) << 8);
+    Flags			= Buffer[3];
+    PacketInterval	= Buffer[4];
+    SampleRate		= Buffer[5] | (UInt16(Buffer[6]) << 8);
+}
+
+SensorFactoryCalibrationImpl::SensorFactoryCalibrationImpl() 
+    : AccelOffset(), GyroOffset(), AccelMatrix(), GyroMatrix(), Temperature(0)
+{
+    memset(Buffer, 0, PacketSize);
+    Buffer[0] = 3;
+}
+
+void SensorFactoryCalibrationImpl::Pack()
+{
+    SInt32 x, y, z;
+
+    Buffer[0] = 3;
+
+    x = SInt32(AccelOffset.x * 1e4f);
+    y = SInt32(AccelOffset.y * 1e4f);
+    z = SInt32(AccelOffset.z * 1e4f);
+    PackSensor(Buffer + 3, x, y, z);
+
+    x = SInt32(GyroOffset.x * 1e4f);
+    y = SInt32(GyroOffset.y * 1e4f);
+    z = SInt32(GyroOffset.z * 1e4f);
+    PackSensor(Buffer + 11, x, y, z);
+
+    // ignore the scale matrices for now
+}
+
+void SensorFactoryCalibrationImpl::Unpack()
+{
+    static const float sensorMax = (1 << 20) - 1;
+    SInt32 x, y, z;
+
+    UnpackSensor(Buffer + 3, &x, &y, &z);
+    AccelOffset.y = (float) y * 1e-4f;
+    AccelOffset.z = (float) z * 1e-4f;
+    AccelOffset.x = (float) x * 1e-4f;
+
+    UnpackSensor(Buffer + 11, &x, &y, &z);
+    GyroOffset.x = (float) x * 1e-4f;
+    GyroOffset.y = (float) y * 1e-4f;
+    GyroOffset.z = (float) z * 1e-4f;
+
+    for (int i = 0; i < 3; i++)
+    {
+        UnpackSensor(Buffer + 19 + 8 * i, &x, &y, &z);
+        AccelMatrix.M[i][0] = (float) x / sensorMax;
+        AccelMatrix.M[i][1] = (float) y / sensorMax;
+        AccelMatrix.M[i][2] = (float) z / sensorMax;
+        AccelMatrix.M[i][i] += 1.0f;
+    }
+
+    for (int i = 0; i < 3; i++)
+    {
+        UnpackSensor(Buffer + 43 + 8 * i, &x, &y, &z);
+        GyroMatrix.M[i][0] = (float) x / sensorMax;
+        GyroMatrix.M[i][1] = (float) y / sensorMax;
+        GyroMatrix.M[i][2] = (float) z / sensorMax;
+        GyroMatrix.M[i][i] += 1.0f;
+    }
+
+    Temperature = (float) Alg::DecodeSInt16(Buffer + 67) / 100.0f;
+}
+
+SensorKeepAliveImpl::SensorKeepAliveImpl(UInt16 interval, UInt16 commandId)
+    : CommandId(commandId), KeepAliveIntervalMs(interval)
+{
+    Pack();
+}
+
+void SensorKeepAliveImpl::Pack()
+{
+    Buffer[0] = 8;
+    Buffer[1] = UByte(CommandId & 0xFF);
+    Buffer[2] = UByte(CommandId >> 8);
+    Buffer[3] = UByte(KeepAliveIntervalMs & 0xFF);
+    Buffer[4] = UByte(KeepAliveIntervalMs >> 8);
+}
+
+void SensorKeepAliveImpl::Unpack()
+{
+    CommandId          = Buffer[1] | (UInt16(Buffer[2]) << 8);
+    KeepAliveIntervalMs= Buffer[3] | (UInt16(Buffer[4]) << 8);
+}
+
+} // namespace OVR