Optimize various engine code

Source/Engine/Core/Math/Transform.cpp

@@ -73,23 +73,59 @@ Transform Transform::Subtract(const Transform& other) const
 Transform Transform::LocalToWorld(const Transform& other) const
 {
     Transform result;
-    Quaternion::Multiply(Orientation, other.Orientation, result.Orientation);
-    result.Orientation.Normalize();
-    Vector3::Multiply(Scale, other.Scale, result.Scale);
-    Vector3 tmp = other.Translation * Scale;
-    Vector3::Transform(tmp, Orientation, tmp);
-    Vector3::Add(tmp, Translation, result.Translation);
+    LocalToWorld(other, result);
     return result;
 }
 
 void Transform::LocalToWorld(const Transform& other, Transform& result) const
 {
-    Quaternion::Multiply(Orientation, other.Orientation, result.Orientation);
-    result.Orientation.Normalize();
-    Vector3::Multiply(Scale, other.Scale, result.Scale);
-    Vector3 tmp = other.Translation * Scale;
-    Vector3::Transform(tmp, Orientation, tmp);
-    Vector3::Add(tmp, Translation, result.Translation);
+    //Quaternion::Multiply(Orientation, other.Orientation, result.Orientation);
+    const float a = Orientation.Y * other.Orientation.Z - Orientation.Z * other.Orientation.Y;
+    const float b = Orientation.Z * other.Orientation.X - Orientation.X * other.Orientation.Z;
+    const float c = Orientation.X * other.Orientation.Y - Orientation.Y * other.Orientation.X;
+    const float d = Orientation.X * other.Orientation.X + Orientation.Y * other.Orientation.Y + Orientation.Z * other.Orientation.Z;
+    result.Orientation.X = Orientation.X * other.Orientation.W + other.Orientation.X * Orientation.W + a;
+    result.Orientation.Y = Orientation.Y * other.Orientation.W + other.Orientation.Y * Orientation.W + b;
+    result.Orientation.Z = Orientation.Z * other.Orientation.W + other.Orientation.Z * Orientation.W + c;
+    result.Orientation.W = Orientation.W * other.Orientation.W - d;
+
+    //result.Orientation.Normalize();
+    const float length = result.Orientation.Length();
+    if (length > ZeroTolerance)
+    {
+        const float inv = 1.0f / length;
+        result.Orientation.X *= inv;
+        result.Orientation.Y *= inv;
+        result.Orientation.Z *= inv;
+        result.Orientation.W *= inv;
+    }
+
+    //Vector3::Multiply(Scale, other.Scale, result.Scale);
+    result.Scale = Vector3(Scale.X * other.Scale.X, Scale.Y * other.Scale.Y, Scale.Z * other.Scale.Z);
+
+    //Vector3 tmp; Vector3::Multiply(other.Translation, Scale, tmp);
+    Vector3 tmp = Vector3(other.Translation.X * Scale.X, other.Translation.Y * Scale.Y, other.Translation.Z * Scale.Z);
+
+    //Vector3::Transform(tmp, Orientation, tmp);
+    const float x = Orientation.X + Orientation.X;
+    const float y = Orientation.Y + Orientation.Y;
+    const float z = Orientation.Z + Orientation.Z;
+    const float wx = Orientation.W * x;
+    const float wy = Orientation.W * y;
+    const float wz = Orientation.W * z;
+    const float xx = Orientation.X * x;
+    const float xy = Orientation.X * y;
+    const float xz = Orientation.X * z;
+    const float yy = Orientation.Y * y;
+    const float yz = Orientation.Y * z;
+    const float zz = Orientation.Z * z;
+    tmp = Vector3(
+        tmp.X * (1.0f - yy - zz) + tmp.Y * (xy - wz) + tmp.Z * (xz + wy),
+        tmp.X * (xy + wz) + tmp.Y * (1.0f - xx - zz) + tmp.Z * (yz - wx),
+        tmp.X * (xz - wy) + tmp.Y * (yz + wx) + tmp.Z * (1.0f - xx - yy));
+
+    //Vector3::Add(tmp, Translation, result.Translation);
+    result.Translation = Vector3(tmp.X + Translation.X, tmp.Y + Translation.Y, tmp.Z + Translation.Z);
 }
 
 Vector3 Transform::LocalToWorld(const Vector3& point) const