1 files changed, 436 insertions, 0 deletions
diff --git a/externals/g3dlite/CoordinateFrame.cpp b/externals/g3dlite/CoordinateFrame.cpp
new file mode 100644
index 00000000000..9b639b62082
--- /dev/null
+++ b/externals/g3dlite/CoordinateFrame.cpp
@@ -0,0 +1,436 @@
+/**
+ @file CoordinateFrame.cpp
+
+ Coordinate frame class
+
+ @maintainer Morgan McGuire, http://graphics.cs.williams.edu
+
+ @created 2001-06-02
+ @edited  2009-11-13
+
+ Copyright 2000-2010, Morgan McGuire.
+ All rights reserved.
+*/
+
+#include "G3D/platform.h"
+#include "G3D/CoordinateFrame.h"
+#include "G3D/Quat.h"
+#include "G3D/Matrix4.h"
+#include "G3D/Box.h"
+#include "G3D/AABox.h"
+#include "G3D/Sphere.h"
+#include "G3D/Triangle.h"
+#include "G3D/Ray.h"
+#include "G3D/Capsule.h"
+#include "G3D/Cylinder.h"
+#include "G3D/UprightFrame.h"
+#include "G3D/Any.h"
+#include "G3D/stringutils.h"
+
+namespace G3D {
+
+CoordinateFrame::CoordinateFrame(const Any& any) {
+    any.verifyName("CFrame");
+    if (toUpper(any.name()) == "CFRAME") {
+        any.verifyType(Any::TABLE, Any::ARRAY);
+        if (any.type() == Any::TABLE) {
+            rotation    = any["rotation"];
+            translation = any["translation"];
+        } else {
+            any.verifySize(2);
+            rotation    = any[0];
+            translation = any[1];
+        }
+    } else {
+        any.verifyName("CFrame::fromXYZYPRDegrees");
+        any.verifyType(Any::ARRAY);
+        any.verifySize(3, 6);
+
+        int s = any.size();
+
+        *this = fromXYZYPRDegrees(any[0], any[1], any[2], 
+                                  (s > 3) ? any[3].number() : 0.0f,
+                                  (s > 4) ? any[4].number() : 0.0f,
+                                  (s > 5) ? any[5].number() : 0.0f);
+    }
+}
+
+
+CoordinateFrame::operator Any() const {
+    float x, y, z, yaw, pitch, roll;
+    getXYZYPRDegrees(x, y, z, yaw, pitch, roll); 
+    Any a(Any::ARRAY, "CFrame::fromXYZYPRDegrees");
+    a.append(x, y, z, yaw);
+    if ( ! G3D::fuzzyEq(yaw, 0.0f) || ! G3D::fuzzyEq(pitch, 0.0f) || ! G3D::fuzzyEq(roll, 0.0f)) {
+        a.append(yaw);
+        if (! G3D::fuzzyEq(pitch, 0.0f) || ! G3D::fuzzyEq(roll, 0.0f)) {
+            a.append(pitch);
+            if (! G3D::fuzzyEq(roll, 0.0f)) {
+                a.append(roll);
+            }
+        }
+    }
+    return a;
+}
+
+
+CoordinateFrame::CoordinateFrame(const class UprightFrame& f) {
+    *this = f.toCoordinateFrame();
+}
+
+
+CoordinateFrame::CoordinateFrame() : 
+    rotation(Matrix3::identity()), translation(Vector3::zero()) {
+}
+
+CoordinateFrame CoordinateFrame::fromXYZYPRRadians(float x, float y, float z, float yaw, 
+                                                   float pitch, float roll) {
+    Matrix3 rotation = Matrix3::fromAxisAngle(Vector3::unitY(), yaw);
+    
+    rotation = Matrix3::fromAxisAngle(rotation.column(0), pitch) * rotation;
+    rotation = Matrix3::fromAxisAngle(rotation.column(2), roll) * rotation;
+
+    const Vector3 translation(x, y, z);
+    
+    return CoordinateFrame(rotation, translation);
+}
+
+
+void CoordinateFrame::getXYZYPRRadians(float& x, float& y, float& z, 
+                                       float& yaw, float& pitch, float& roll) const {
+    x = translation.x;
+    y = translation.y;
+    z = translation.z;
+    
+    const Vector3& look = lookVector();
+
+    if (abs(look.y) > 0.99f) {
+        // Looking nearly straight up or down
+
+        yaw   = G3D::pi() + atan2(look.x, look.z);
+        pitch = asin(look.y);
+        roll  = 0.0f;
+        
+    } else {
+
+        // Yaw cannot be affected by others, so pull it first
+        yaw = G3D::pi() + atan2(look.x, look.z);
+        
+        // Pitch is the elevation of the yaw vector
+        pitch = asin(look.y);
+        
+        Vector3 actualRight = rightVector();
+        Vector3 expectedRight = look.cross(Vector3::unitY());
+
+        roll = 0;//acos(actualRight.dot(expectedRight));  TODO
+    }
+}
+
+
+void CoordinateFrame::getXYZYPRDegrees(float& x, float& y, float& z, 
+                                       float& yaw, float& pitch, float& roll) const {
+    getXYZYPRRadians(x, y, z, yaw, pitch, roll);
+    yaw   = toDegrees(yaw);
+    pitch = toDegrees(pitch);
+    roll  = toDegrees(roll);
+}
+    
+
+CoordinateFrame CoordinateFrame::fromXYZYPRDegrees(float x, float y, float z, 
+                                                   float yaw, float pitch, float roll) {
+    return fromXYZYPRRadians(x, y, z, toRadians(yaw), toRadians(pitch), toRadians(roll));
+}
+
+
+Ray CoordinateFrame::lookRay() const {
+    return Ray::fromOriginAndDirection(translation, lookVector());
+}
+
+
+bool CoordinateFrame::fuzzyEq(const CoordinateFrame& other) const {
+
+    for (int c = 0; c < 3; ++c) {
+        for (int r = 0; r < 3; ++r) {
+            if (! G3D::fuzzyEq(other.rotation[r][c], rotation[r][c])) {
+                return false;
+            }
+        }
+        if (! G3D::fuzzyEq(translation[c], other.translation[c])) {
+            return false;
+        }
+    }
+
+    return true;
+}
+
+
+bool CoordinateFrame::fuzzyIsIdentity() const {
+    const Matrix3& I = Matrix3::identity();
+
+    for (int c = 0; c < 3; ++c) {
+        for (int r = 0; r < 3; ++r) {
+            if (fuzzyNe(I[r][c], rotation[r][c])) {
+                return false;
+            }
+        }
+        if (fuzzyNe(translation[c], 0)) {
+            return false;
+        }
+    }
+
+    return true;
+}
+
+
+bool CoordinateFrame::isIdentity() const {
+    return 
+        (translation == Vector3::zero()) &&
+        (rotation == Matrix3::identity());
+}
+
+
+Matrix4 CoordinateFrame::toMatrix4() const {
+    return Matrix4(*this);
+}
+
+
+std::string CoordinateFrame::toXML() const {
+    return G3D::format(
+        "<COORDINATEFRAME>\n  %lf,%lf,%lf,%lf,\n  %lf,%lf,%lf,%lf,\n  %lf,%lf,%lf,%lf,\n  %lf,%lf,%lf,%lf\n</COORDINATEFRAME>\n",
+        rotation[0][0], rotation[0][1], rotation[0][2], translation.x,
+        rotation[1][0], rotation[1][1], rotation[1][2], translation.y,
+        rotation[2][0], rotation[2][1], rotation[2][2], translation.z,
+        0.0, 0.0, 0.0, 1.0);
+}
+
+
+Plane CoordinateFrame::toObjectSpace(const Plane& p) const {
+    Vector3 N, P;
+    double d;
+    p.getEquation(N, d);
+    P = N * (float)d;
+    P = pointToObjectSpace(P);
+    N = normalToObjectSpace(N);
+    return Plane(N, P);
+}
+
+
+Plane CoordinateFrame::toWorldSpace(const Plane& p) const {
+    Vector3 N, P;
+    double d;
+    p.getEquation(N, d);
+    P = N * (float)d;
+    P = pointToWorldSpace(P);
+    N = normalToWorldSpace(N);
+    return Plane(N, P);
+}
+
+
+Triangle CoordinateFrame::toObjectSpace(const Triangle& t) const {
+    return Triangle(pointToObjectSpace(t.vertex(0)),
+        pointToObjectSpace(t.vertex(1)),
+        pointToObjectSpace(t.vertex(2)));
+}
+
+
+Triangle CoordinateFrame::toWorldSpace(const Triangle& t) const {
+    return Triangle(pointToWorldSpace(t.vertex(0)),
+        pointToWorldSpace(t.vertex(1)),
+        pointToWorldSpace(t.vertex(2)));
+}
+
+
+Cylinder CoordinateFrame::toWorldSpace(const Cylinder& c) const {
+    return Cylinder(
+        pointToWorldSpace(c.point(0)), 
+        pointToWorldSpace(c.point(1)), 
+        c.radius());
+}
+
+
+Capsule CoordinateFrame::toWorldSpace(const Capsule& c) const {
+    return Capsule(
+        pointToWorldSpace(c.point(0)), 
+        pointToWorldSpace(c.point(1)), 
+        c.radius());
+}
+
+
+Box CoordinateFrame::toWorldSpace(const AABox& b) const {
+    Box b2(b);
+    return toWorldSpace(b2);
+}
+
+
+Box CoordinateFrame::toWorldSpace(const Box& b) const {
+    Box out(b);
+
+    for (int i = 0; i < 8; ++i) {
+        out._corner[i] = pointToWorldSpace(b._corner[i]);
+        debugAssert(! isNaN(out._corner[i].x));
+    }
+
+    for (int i = 0; i < 3; ++i) {
+        out._axis[i] = vectorToWorldSpace(b._axis[i]);
+    }
+
+    out._center = pointToWorldSpace(b._center);
+
+    return out;
+}
+
+
+Box CoordinateFrame::toObjectSpace(const Box &b) const {
+    return inverse().toWorldSpace(b);
+}
+
+
+Box CoordinateFrame::toObjectSpace(const AABox& b) const {
+    return toObjectSpace(Box(b));
+}
+
+
+CoordinateFrame::CoordinateFrame(class BinaryInput& b) : rotation(Matrix3::zero()) {
+    deserialize(b);
+}
+
+
+void CoordinateFrame::deserialize(class BinaryInput& b) {
+    rotation.deserialize(b);
+    translation.deserialize(b);
+}
+
+
+void CoordinateFrame::serialize(class BinaryOutput& b) const {
+    rotation.serialize(b);
+    translation.serialize(b);
+}
+
+
+Sphere CoordinateFrame::toWorldSpace(const Sphere &b) const {
+    return Sphere(pointToWorldSpace(b.center), b.radius);
+}
+
+
+Sphere CoordinateFrame::toObjectSpace(const Sphere &b) const {
+    return Sphere(pointToObjectSpace(b.center), b.radius);
+}
+
+
+Ray CoordinateFrame::toWorldSpace(const Ray& r) const {
+    return Ray::fromOriginAndDirection(pointToWorldSpace(r.origin()), vectorToWorldSpace(r.direction()));
+}
+
+
+Ray CoordinateFrame::toObjectSpace(const Ray& r) const {
+    return Ray::fromOriginAndDirection(pointToObjectSpace(r.origin()), vectorToObjectSpace(r.direction()));
+}
+
+
+void CoordinateFrame::lookAt(const Vector3 &target) {
+    lookAt(target, Vector3::unitY());
+}
+
+
+void CoordinateFrame::lookAt(
+    const Vector3&  target,
+    Vector3         up) {
+
+    up = up.direction();
+
+    Vector3 look = (target - translation).direction();
+    if (fabs(look.dot(up)) > .99f) {
+        up = Vector3::unitX();
+        if (fabs(look.dot(up)) > .99f) {
+            up = Vector3::unitY();
+        }
+    }
+
+    up -= look * look.dot(up);
+    up.unitize();
+
+    Vector3 z = -look;
+    Vector3 x = -z.cross(up);
+    x.unitize();
+
+    Vector3 y = z.cross(x);
+
+    rotation.setColumn(0, x);
+    rotation.setColumn(1, y);
+    rotation.setColumn(2, z);
+}
+
+
+CoordinateFrame CoordinateFrame::lerp(
+    const CoordinateFrame&  other,
+    float                   alpha) const {
+
+    if (alpha == 1.0f) {
+        return other;
+    } else if (alpha == 0.0f) {
+        return *this;
+    } else {
+        Quat q1 = Quat(this->rotation);
+        Quat q2 = Quat(other.rotation);
+
+        return CoordinateFrame(
+            q1.slerp(q2, alpha).toRotationMatrix(),
+            this->translation * (1 - alpha) + other.translation * alpha);
+    }
+} 
+
+
+void CoordinateFrame::pointToWorldSpace(const Array<Vector3>& v, Array<Vector3>& vout) const {
+    vout.resize(v.size());
+
+    for (int i = v.size() - 1; i >= 0; --i) {
+        vout[i] = pointToWorldSpace(v[i]);
+    }
+}
+
+
+void CoordinateFrame::normalToWorldSpace(const Array<Vector3>& v, Array<Vector3>& vout) const  {
+    vout.resize(v.size());
+
+    for (int i = v.size() - 1; i >= 0; --i) {
+        vout[i] = normalToWorldSpace(v[i]);
+    }
+}
+
+
+void CoordinateFrame::vectorToWorldSpace(const Array<Vector3>& v, Array<Vector3>& vout) const {
+    vout.resize(v.size());
+
+    for (int i = v.size() - 1; i >= 0; --i) {
+        vout[i] = vectorToWorldSpace(v[i]);
+    }
+}
+
+
+void CoordinateFrame::pointToObjectSpace(const Array<Vector3>& v, Array<Vector3>& vout) const {
+    vout.resize(v.size());
+
+    for (int i = v.size() - 1; i >= 0; --i) {
+        vout[i] = pointToObjectSpace(v[i]);
+    }
+}
+
+
+void CoordinateFrame::normalToObjectSpace(const Array<Vector3>& v, Array<Vector3>& vout) const {
+    vout.resize(v.size());
+
+    for (int i = v.size() - 1; i >= 0; --i) {
+        vout[i] = normalToObjectSpace(v[i]);
+    }
+}
+
+
+void CoordinateFrame::vectorToObjectSpace(const Array<Vector3>& v, Array<Vector3>& vout) const {
+    vout.resize(v.size());
+
+    for (int i = v.size() - 1; i >= 0; --i) {
+        vout[i] = vectorToObjectSpace(v[i]);
+    }
+}
+
+} // namespace