Jim

src/J3ML/LinearAlgebra/Matrix3x3.cpp

@@ -196,5 +196,87 @@ namespace LinearAlgebra {
         };
     }
 
+    void Matrix3x3::SetRotatePart(const Vector3 &a, float angle) {
+        float s = std::sin(angle);
+        float c = std::cos(angle);
+
+        const float c1 = 1.f - c;
+
+        elems[0][0] = c+c1*a.x*a.x;
+        elems[1][0] = c1*a.x*a.y+s*a.z;
+        elems[2][0] = c1*a.x*a.z-s*a.y;
+
+        elems[0][1] = c1*a.x*a.y-s*a.z;
+        elems[1][1] = c+c1*a.y*a.y;
+        elems[2][1] = c1*a.y*a.z+s*a.x;
+
+        elems[0][2] = c1*a.x*a.z+s*a.y;
+        elems[1][2] = c1*a.y*a.z-s*a.x;
+        elems[2][2] = c+c1*a.z*a.z;
+
+    }
+
+    Matrix3x3 Matrix3x3::RotateAxisAngle(const Vector3 &axis, float angleRadians) {
+        Matrix3x3 r;
+        r.SetRotatePart(axis, angleRadians);
+        return r;
+    }
+
+    void Matrix3x3::SetRow(int i, const Vector3 &vec) {
+        elems[i][0] = vec.x;
+        elems[i][1] = vec.y;
+        elems[i][2] = vec.z;
+    }
+
+    Matrix3x3
+    Matrix3x3::LookAt(const Vector3 &forward, const Vector3 &target, const Vector3 &localUp, const Vector3 &worldUp) {
+        // User must input proper normalized input direction vectors
+        // In the local space, the forward and up directions must be perpendicular to be well-formed.
+        // In the world space, the target direction and world up cannot be degenerate (co-linear)
+        // Generate the third basis vector in the local space;
+        Vector3 localRight = localUp.Cross(forward).Normalize();
+        // A. Now we have an orthonormal linear basis {localRight, localUp, forward} for the object local space.
+        // Generate the third basis vector for the world space
+        Vector3 worldRight = worldUp.Cross(target).Normalize();
+        // Since the input worldUp vector is not necessarily perpendicular to the target direction vector
+        // We need to compute the real world space up vector that the "head" of the object will point
+        // towards when the model is looking towards the desired target direction
+        Vector3 perpWorldUp = target.Cross(worldRight).Normalize();
+        // B. Now we have an orthonormal linear basis {worldRight, perpWorldUp, targetDirection } for the desired target orientation.
+        // We want to build a matrix M that performs the following mapping:
+        // 1. localRight must be mapped to worldRight.        (M * localRight = worldRight)
+        // 2. localUp must be mapped to perpWorldUp.          (M * localUp = perpWorldUp)
+        // 3. localForward must be mapped to targetDirection. (M * localForward = targetDirection)
+        // i.e. we want to map the basis A to basis B.
+
+        // This matrix M exists, and it is an orthonormal rotation matrix with a determinant of +1, because
+        // the bases A and B are orthonormal with the same handedness.
+
+        // Below, use the notation that (a,b,c) is a 3x3 matrix with a as its first column, b second, and c third.
+
+        // By algebraic manipulation, we can rewrite conditions 1, 2 and 3 in a matrix form:
+        //        M * (localRight, localUp, localForward) = (worldRight, perpWorldUp, targetDirection)
+        // or     M = (worldRight, perpWorldUp, targetDirection) * (localRight, localUp, localForward)^{-1}.
+        // or     M = m1 * m2, where
+
+        // m1 equals (worldRight, perpWorldUp, target):
+        Matrix3x3 m1(worldRight, perpWorldUp, target);
+
+        // and m2 equals (localRight, localUp, localForward)^{-1}:
+        Matrix3x3 m2;
+        m2.SetRow(0, localRight);
+        m2.SetRow(1, localUp);
+        m2.SetRow(2, forward);
+        // Above we used the shortcut that for an orthonormal matrix M, M^{-1} = M^T. So set the rows
+        // and not the columns to directly produce the transpose, i.e. the inverse of (localRight, localUp, localForward).
+
+        // Compute final M.
+        m2 = m1 * m2;
+
+        // And fix any numeric stability issues by re-orthonormalizing the result.
+        m2.Orthonormalize(0, 1, 2);
+        return m2;
+    }
+
 }