1 files changed, 132 insertions, 0 deletions
diff --git a/externals/g3dlite/UprightFrame.cpp b/externals/g3dlite/UprightFrame.cpp
new file mode 100644
index 00000000000..c80264bf4e8
--- /dev/null
+++ b/externals/g3dlite/UprightFrame.cpp
@@ -0,0 +1,132 @@
+/**
+  @file UprightFrame.cpp
+  Box class
+
+  @maintainer Morgan McGuire, http://graphics.cs.williams.edu
+
+  @created 2007-05-02
+  @edited  2007-05-05
+*/
+
+#include "G3D/UprightFrame.h"
+#include "G3D/BinaryInput.h"
+#include "G3D/BinaryOutput.h"
+
+namespace G3D {
+
+UprightFrame::UprightFrame(const CoordinateFrame& cframe) {
+    Vector3 look = cframe.lookVector();
+
+    yaw = G3D::pi() + atan2(look.x, look.z);
+    pitch = asin(look.y);
+
+    translation = cframe.translation;
+}
+
+    
+CoordinateFrame UprightFrame::toCoordinateFrame() const {
+    CoordinateFrame cframe;
+
+    Matrix3 P(Matrix3::fromAxisAngle(Vector3::unitX(), pitch));
+    Matrix3 Y(Matrix3::fromAxisAngle(Vector3::unitY(), yaw));
+
+    cframe.rotation = Y * P;
+    cframe.translation = translation;
+
+    return cframe;
+}
+
+
+UprightFrame UprightFrame::operator+(const UprightFrame& other) const {
+    return UprightFrame(translation + other.translation, pitch + other.pitch, yaw + other.yaw);
+}
+
+
+UprightFrame UprightFrame::operator*(const float k) const {
+    return UprightFrame(translation * k, pitch * k, yaw * k);
+}
+
+
+void UprightFrame::unwrapYaw(UprightFrame* a, int N) {
+    // Use the first point to establish the wrapping convention
+    for (int i = 1; i < N; ++i) {
+        const float prev = a[i - 1].yaw;
+        float& cur = a[i].yaw;
+
+        // No two angles should be more than pi (i.e., 180-degrees) apart.  
+        if (abs(cur - prev) > G3D::pi()) {
+            // These angles must have wrapped at zero, causing them
+            // to be interpolated the long way.
+
+            // Find canonical [0, 2pi] versions of these numbers
+            float p = wrap(prev, twoPi());
+            float c = wrap(cur, twoPi());
+            
+            // Find the difference -pi < diff < pi between the current and previous values
+            float diff = c - p;
+            if (diff < -G3D::pi()) {
+                diff += twoPi();
+            } else if (diff > G3D::pi()) {
+                diff -= twoPi();
+            } 
+
+            // Offset the current from the previous by the difference
+            // between them.
+            cur = prev + diff;
+        }
+    }
+}
+
+
+void UprightFrame::serialize(class BinaryOutput& b) const {
+    translation.serialize(b);
+    b.writeFloat32(pitch);
+    b.writeFloat32(yaw);
+}
+
+
+void UprightFrame::deserialize(class BinaryInput& b) {
+    translation.deserialize(b);
+    pitch = b.readFloat32();
+    yaw = b.readFloat32();
+}
+
+
+void UprightSpline::serialize(class BinaryOutput& b) const {
+    b.writeBool8(cyclic);
+
+    b.writeInt32(control.size());
+    for (int i = 0; i < control.size(); ++i) {
+        control[i].serialize(b);
+    }
+    b.writeInt32(time.size());
+    for (int i = 0; i < time.size(); ++i) {
+        b.writeFloat32(time[i]);
+    }
+}
+
+
+void UprightSpline::deserialize(class BinaryInput& b) {
+    cyclic = b.readBool8();
+
+    control.resize(b.readInt32());
+    for (int i = 0; i < control.size(); ++i) {
+        control[i].deserialize(b);
+    }
+
+    if (b.hasMore()) {
+        time.resize(b.readInt32());
+        for (int i = 0; i < time.size(); ++i) {
+            time[i] = b.readFloat32();
+        }
+        debugAssert(time.size() == control.size());
+    } else {
+        // Import legacy path
+        time.resize(control.size());
+        for (int i = 0; i < time.size(); ++i) {
+            time[i] = i;
+        }
+    }
+}
+
+}