3 files changed, 26 insertions, 4 deletions
diff --git a/src/math/Vector.cpp b/src/math/Vector.cpp
index ee76e555..e29d4335 100644
--- a/src/math/Vector.cpp
+++ b/src/math/Vector.cpp
@@ -44,3 +44,18 @@ operator*(const CMatrix &mat, const CVector &vec)
 	               mat.ry * vec.x + mat.fy * vec.y + mat.uy * vec.z + mat.py,
 	               mat.rz * vec.x + mat.fz * vec.y + mat.uz * vec.z + mat.pz);
 }
+
+void
+RwV3dTransformPoints(CVector * pointsOut, const CVector * pointsIn, RwInt32 numPoints, const RwMatrix * matrix)
+{
+	while(numPoints--){
+		float x = pointsIn->x*matrix->right.x + pointsIn->y*matrix->up.x + pointsIn->z*matrix->at.x + matrix->pos.x;
+		float y = pointsIn->x*matrix->right.y + pointsIn->y*matrix->up.y + pointsIn->z*matrix->at.y + matrix->pos.y;
+		float z = pointsIn->x*matrix->right.z + pointsIn->y*matrix->up.z + pointsIn->z*matrix->at.z + matrix->pos.z;
+		pointsOut->x = x;
+		pointsOut->y = y;
+		pointsOut->z = z;
+		pointsOut++;
+		pointsIn++;
+	}
+}
diff --git a/src/math/Vector.h b/src/math/Vector.h
index 02128454..a86e4e72 100644
--- a/src/math/Vector.h
+++ b/src/math/Vector.h
@@ -1,8 +1,12 @@
 #pragma once
 
+// TODO(LCS): this should have 16 byte alignment but VS doesn't like passing aligned values by value
+// need a solution for this eventually if we ever want to load original assets
 class CVector : public RwV3d
 {
 public:
+	float w;
+
 	CVector(void) {}
 	CVector(float x, float y, float z)
 	{
@@ -18,7 +22,7 @@ public:
 		z = v.z;
 	}
 	// (0,1,0) means no rotation. So get right vector and its atan
-	float Heading(void) const { return Atan2(-x, y); }
+	float Heading(void) const { return x == 0.0f && y == 0.0f ? 0.0f : Atan2(-x, y); }
 	float Magnitude(void) const { return Sqrt(x*x + y*y + z*z); }
 	float MagnitudeSqr(void) const { return x*x + y*y + z*z; }
 	float Magnitude2D(void) const { return Sqrt(x*x + y*y); }
@@ -126,4 +130,7 @@ class CMatrix;
 
 CVector Multiply3x3(const CMatrix &mat, const CVector &vec);
 CVector Multiply3x3(const CVector &vec, const CMatrix &mat);
-CVector operator*(const CMatrix &mat, const CVector &vec);
-\ No newline at end of file
+CVector operator*(const CMatrix &mat, const CVector &vec);
+
+// we need this because CVector and RwV3d have different size now
+void RwV3dTransformPoints(CVector * pointsOut, const CVector * pointsIn, RwInt32 numPoints, const RwMatrix * matrix);
diff --git a/src/math/VuVector.h b/src/math/VuVector.h
index 41584095..ccaa4cfb 100644
--- a/src/math/VuVector.h
+++ b/src/math/VuVector.h
@@ -3,10 +3,10 @@
 class TYPEALIGN(16) CVuVector : public CVector
 {
 public:
-	float w;
+//	float w;	// in CVector now
 	CVuVector(void) {}
 	CVuVector(float x, float y, float z) : CVector(x, y, z) {}
-	CVuVector(float x, float y, float z, float w) : CVector(x, y, z), w(w) {}
+	CVuVector(float x, float y, float z, float w) : CVector(x, y, z)/*, w(w)*/ { this->w = w;}
 	CVuVector(const CVector &v) : CVector(v.x, v.y, v.z) {}
 	CVuVector(const RwV3d &v) : CVector(v) {}
 /*