Fixed all functionality hidden under a debug macro in double precision mode (#379)

Jolt/Geometry/ConvexHullBuilder.cpp

@@ -112,7 +112,7 @@ ConvexHullBuilder::ConvexHullBuilder(const Positions &inPositions) :
 	mIteration = 0;
 
 	// Center the drawing of the first hull around the origin and calculate the delta offset between states
-	mOffset = Vec3::sZero();
+	mOffset = RVec3::sZero();
 	if (mPositions.empty())
 	{
 		// No hull will be generated
@@ -127,7 +127,7 @@ ConvexHullBuilder::ConvexHullBuilder(const Positions &inPositions) :
 			maxv = Vec3::sMax(maxv, v);
 			mOffset -= v;
 		}
-		mOffset /= float(mPositions.size());
+		mOffset /= Real(mPositions.size());
 		mDelta = Vec3((maxv - minv).GetX() + 0.5f, 0, 0);
 		mOffset += mDelta; // Don't start at origin, we're already drawing the final hull there
 	}
@@ -858,7 +858,7 @@ void ConvexHullBuilder::FindEdge(Face *inFacingFace, Vec3Arg inVertex, FullEdges
 #ifdef JPH_CONVEX_BUILDER_DEBUG
 	// Draw edge of facing faces
 	for (int i = 0; i < (int)outEdges.size(); ++i)
-		DebugRenderer::sInstance->DrawArrow(cDrawScale * (mPositions[outEdges[i].mStartIdx] + mOffset), cDrawScale * (mPositions[outEdges[i].mEndIdx] + mOffset), Color::sWhite, 0.01f);
+		DebugRenderer::sInstance->DrawArrow(cDrawScale * (mOffset + mPositions[outEdges[i].mStartIdx]), cDrawScale * (mOffset + mPositions[outEdges[i].mEndIdx]), Color::sWhite, 0.01f);
 	DrawState();
 #endif
 }
@@ -1380,11 +1380,11 @@ void ConvexHullBuilder::DrawState(bool inDrawConflictList) const
 
 			// First point
 			const Edge *e = f->mFirstEdge;
-			Vec3 p1 = cDrawScale * (mPositions[e->mStartIdx] + mOffset);
+			RVec3 p1 = cDrawScale * (mOffset + mPositions[e->mStartIdx]);
 
 			// Second point
 			e = e->mNextEdge;
-			Vec3 p2 = cDrawScale * (mPositions[e->mStartIdx] + mOffset);
+			RVec3 p2 = cDrawScale * (mOffset + mPositions[e->mStartIdx]);
 
 			// First line
 			DebugRenderer::sInstance->DrawLine(p1, p2, Color::sGrey);
@@ -1393,7 +1393,7 @@ void ConvexHullBuilder::DrawState(bool inDrawConflictList) const
 			{
 				// Third point
 				e = e->mNextEdge;
-				Vec3 p3 = cDrawScale * (mPositions[e->mStartIdx] + mOffset);
+				RVec3 p3 = cDrawScale * (mOffset + mPositions[e->mStartIdx]);
 
 				DebugRenderer::sInstance->DrawTriangle(p1, p2, p3, iteration_color);
 
@@ -1404,7 +1404,7 @@ void ConvexHullBuilder::DrawState(bool inDrawConflictList) const
 			while (e != f->mFirstEdge);
 
 			// Draw normal
-			Vec3 centroid = cDrawScale * (f->mCentroid + mOffset);
+			RVec3 centroid = cDrawScale * (mOffset + f->mCentroid);
 			DebugRenderer::sInstance->DrawArrow(centroid, centroid + f->mNormal.NormalizedOr(Vec3::sZero()), face_color, 0.01f);
 
 			// Draw conflict list
@@ -1420,11 +1420,11 @@ void ConvexHullBuilder::DrawState(bool inDrawConflictList) const
 void ConvexHullBuilder::DrawWireFace(const Face *inFace, ColorArg inColor) const
 {
 	const Edge *e = inFace->mFirstEdge;
-	Vec3 prev = cDrawScale * (mPositions[e->mStartIdx] + mOffset);
+	RVec3 prev = cDrawScale * (mOffset + mPositions[e->mStartIdx]);
 	do
 	{ 
 		const Edge *next = e->mNextEdge;
-		Vec3 cur = cDrawScale * (mPositions[next->mStartIdx] + mOffset);
+		RVec3 cur = cDrawScale * (mOffset + mPositions[next->mStartIdx]);
 		DebugRenderer::sInstance->DrawArrow(prev, cur, inColor, 0.01f);
 		DebugRenderer::sInstance->DrawText3D(prev, ConvertToString(e->mStartIdx), inColor);
 		e = next;
@@ -1434,8 +1434,8 @@ void ConvexHullBuilder::DrawWireFace(const Face *inFace, ColorArg inColor) const
 
 void ConvexHullBuilder::DrawEdge(const Edge *inEdge, ColorArg inColor) const
 {
-	Vec3 p1 = cDrawScale * (mPositions[inEdge->mStartIdx] + mOffset);
-	Vec3 p2 = cDrawScale * (mPositions[inEdge->mNextEdge->mStartIdx] + mOffset);
+	RVec3 p1 = cDrawScale * (mOffset + mPositions[inEdge->mStartIdx]);
+	RVec3 p2 = cDrawScale * (mOffset + mPositions[inEdge->mNextEdge->mStartIdx]);
 	DebugRenderer::sInstance->DrawArrow(p1, p2, inColor, 0.01f);
 }
 

Jolt/Geometry/ConvexHullBuilder.h

@@ -134,7 +134,7 @@ private:
 
 #ifdef JPH_CONVEX_BUILDER_DEBUG
 	/// Factor to scale convex hull when debug drawing the construction process
-	static constexpr float cDrawScale = 10.0f;
+	static constexpr Real cDrawScale = 10;
 #endif
 
 	/// Class that holds an edge including start and end index
@@ -267,7 +267,7 @@ private:
 
 #ifdef JPH_CONVEX_BUILDER_DEBUG
 	int					mIteration;							///< Number of iterations we've had so far (for debug purposes)	
-	mutable Vec3		mOffset;							///< Offset to use for state drawing
+	mutable RVec3		mOffset;							///< Offset to use for state drawing
 	Vec3				mDelta;								///< Delta offset between next states
 #endif
 };

Jolt/Geometry/ConvexHullBuilder2D.cpp

@@ -33,7 +33,7 @@ ConvexHullBuilder2D::ConvexHullBuilder2D(const Positions &inPositions) :
 {
 #ifdef JPH_CONVEX_BUILDER_2D_DEBUG
 	// Center the drawing of the first hull around the origin and calculate the delta offset between states
-	mOffset = Vec3::sZero();
+	mOffset = RVec3::sZero();
 	if (mPositions.empty())
 	{
 		// No hull will be generated
@@ -48,7 +48,7 @@ ConvexHullBuilder2D::ConvexHullBuilder2D(const Positions &inPositions) :
 			maxv = Vec3::sMax(maxv, v);
 			mOffset -= v;
 		}
-		mOffset /= float(mPositions.size());
+		mOffset /= Real(mPositions.size());
 		mDelta = Vec3((maxv - minv).GetX() + 0.5f, 0, 0);
 		mOffset += mDelta; // Don't start at origin, we're already drawing the final hull there
 	}
@@ -317,8 +317,8 @@ void ConvexHullBuilder2D::DrawState()
 		Color color = Color::sGetDistinctColor(color_idx++);
 
 		// Draw edge and normal
-		DebugRenderer::sInstance->DrawArrow(cDrawScale * (mPositions[edge->mStartIdx] + mOffset), cDrawScale * (mPositions[next->mStartIdx] + mOffset), color, 0.1f);
-		DebugRenderer::sInstance->DrawArrow(cDrawScale * (edge->mCenter + mOffset), cDrawScale * (edge->mCenter + mOffset) + edge->mNormal.NormalizedOr(Vec3::sZero()), Color::sGreen, 0.1f);
+		DebugRenderer::sInstance->DrawArrow(cDrawScale * (mOffset + mPositions[edge->mStartIdx]), cDrawScale * (mOffset + mPositions[next->mStartIdx]), color, 0.1f);
+		DebugRenderer::sInstance->DrawArrow(cDrawScale * (mOffset + edge->mCenter), cDrawScale * (mOffset + edge->mCenter) + edge->mNormal.NormalizedOr(Vec3::sZero()), Color::sGreen, 0.1f);
 
 		// Draw points that belong to this edge in the same color
 		for (int idx : edge->mConflictList)

Jolt/Geometry/ConvexHullBuilder2D.h

@@ -42,7 +42,7 @@ public:
 private:
 #ifdef JPH_CONVEX_BUILDER_2D_DEBUG
 	/// Factor to scale convex hull when debug drawing the construction process
-	static constexpr float cDrawScale = 10.0f;
+	static constexpr Real cDrawScale = 10;
 #endif
 
 	class Edge;
@@ -96,7 +96,7 @@ private:
 	int					mNumEdges = 0;								///< Number of edges in hull
 
 #ifdef JPH_CONVEX_BUILDER_2D_DEBUG
-	Vec3				mOffset;									///< Offset to use for state drawing
+	RVec3				mOffset;									///< Offset to use for state drawing
 	Vec3				mDelta;										///< Delta offset between next states
 #endif
 };

Jolt/Geometry/EPAConvexHullBuilder.h

@@ -24,7 +24,7 @@ class EPAConvexHullBuilder : public NonCopyable
 private:
 #ifdef JPH_EPA_CONVEX_BUILDER_DRAW
 	/// Factor to scale convex hull when debug drawing the construction process
-	static constexpr float cDrawScale = 10.0f;
+	static constexpr Real cDrawScale = 10;
 #endif
 
 public:
@@ -224,7 +224,7 @@ public:
 	{
 #ifdef JPH_EPA_CONVEX_BUILDER_DRAW
 		mIteration = 0;
-		mOffset = Vec3::sZero();
+		mOffset = RVec3::sZero();
 #endif
 	}
 
@@ -545,8 +545,8 @@ private:
 		// Draw edge of facing triangles
 		for (int i = 0; i < (int)outEdges.size(); ++i)
 		{
-			Vec3 edge_start = cDrawScale * (mPositions[outEdges[i].mStartIdx] + mOffset);
-			DebugRenderer::sInstance->DrawArrow(edge_start, cDrawScale * (mPositions[outEdges[(i + 1) % outEdges.size()].mStartIdx] + mOffset), Color::sYellow, 0.01f);
+			RVec3 edge_start = cDrawScale * (mOffset + mPositions[outEdges[i].mStartIdx]);
+			DebugRenderer::sInstance->DrawArrow(edge_start, cDrawScale * (mOffset + mPositions[outEdges[(i + 1) % outEdges.size()].mStartIdx]), Color::sYellow, 0.01f);
 			DebugRenderer::sInstance->DrawText3D(edge_start, ConvertToString(outEdges[i].mStartIdx), Color::sWhite);
 		}
 
@@ -613,23 +613,23 @@ public:
 	void				DrawState()
 	{
 		// Draw origin
-		DebugRenderer::sInstance->DrawCoordinateSystem(Mat44::sTranslation(cDrawScale * mOffset), 1.0f);
+		DebugRenderer::sInstance->DrawCoordinateSystem(RMat44::sTranslation(cDrawScale * mOffset), 1.0f);
 
 		// Draw triangles
 		for (const Triangle *t : mTriangles)
 			if (!t->mRemoved)
 			{
 				// Calculate the triangle vertices
-				Vec3 p1 = cDrawScale * (mPositions[t->mEdge[0].mStartIdx] + mOffset);
-				Vec3 p2 = cDrawScale * (mPositions[t->mEdge[1].mStartIdx] + mOffset);
-				Vec3 p3 = cDrawScale * (mPositions[t->mEdge[2].mStartIdx] + mOffset);
+				RVec3 p1 = cDrawScale * (mOffset + mPositions[t->mEdge[0].mStartIdx]);
+				RVec3 p2 = cDrawScale * (mOffset + mPositions[t->mEdge[1].mStartIdx]);
+				RVec3 p3 = cDrawScale * (mOffset + mPositions[t->mEdge[2].mStartIdx]);
 
 				// Draw triangle
 				DebugRenderer::sInstance->DrawTriangle(p1, p2, p3, Color::sGetDistinctColor(t->mIteration));
 				DebugRenderer::sInstance->DrawWireTriangle(p1, p2, p3, Color::sGrey);
 
 				// Draw normal
-				Vec3 centroid = cDrawScale * (t->mCentroid + mOffset);
+				RVec3 centroid = cDrawScale * (mOffset + t->mCentroid);
 				float len = t->mNormal.Length();
 				if (len > 0.0f)
 					DebugRenderer::sInstance->DrawArrow(centroid, centroid + t->mNormal / len, Color::sDarkGreen, 0.01f);
@@ -651,10 +651,10 @@ public:
 	/// Draw a triangle for debugging purposes
 	void				DrawWireTriangle(const Triangle &inTriangle, ColorArg inColor)
 	{
-		Vec3 prev = cDrawScale * (mPositions[inTriangle.mEdge[2].mStartIdx] + mOffset);
+		RVec3 prev = cDrawScale * (mOffset + mPositions[inTriangle.mEdge[2].mStartIdx]);
 		for (const Edge &edge : inTriangle.mEdge)
 		{
-			Vec3 cur = cDrawScale * (mPositions[edge.mStartIdx] + mOffset);
+			RVec3 cur = cDrawScale * (mOffset + mPositions[edge.mStartIdx]);
 			DebugRenderer::sInstance->DrawArrow(prev, cur, inColor, 0.01f);
 			prev = cur;
 		}
@@ -684,7 +684,7 @@ private:
 
 #ifdef JPH_EPA_CONVEX_BUILDER_DRAW
 	int					mIteration;							///< Number of iterations we've had so far (for debug purposes)	
-	Vec3				mOffset;							///< Offset to use for state drawing
+	RVec3				mOffset;							///< Offset to use for state drawing
 #endif
 };
 

Jolt/Geometry/GJKClosestPoint.h

@@ -918,7 +918,7 @@ private:
 	/// Draw state of algorithm
 	void		DrawState()
 	{
-		Mat44 origin = Mat44::sTranslation(mOffset);
+		RMat44 origin = RMat44::sTranslation(mOffset);
 
 		// Draw origin
 		DebugRenderer::sInstance->DrawCoordinateSystem(origin, 1.0f);
@@ -930,20 +930,20 @@ private:
 		for (int i = 0; i < mNumPoints; ++i)
 		{
 			// Draw support point
-			Vec3 y_i = origin * mY[i];
+			RVec3 y_i = origin * mY[i];
 			DebugRenderer::sInstance->DrawMarker(y_i, Color::sRed, 1.0f);
 			for (int j = i + 1; j < mNumPoints; ++j)
 			{
 				// Draw edge
-				Vec3 y_j = origin * mY[j];
+				RVec3 y_j = origin * mY[j];
 				DebugRenderer::sInstance->DrawLine(y_i, y_j, Color::sRed);
 				for (int k = j + 1; k < mNumPoints; ++k)
 				{
 					// Make sure triangle faces the origin
-					Vec3 y_k = origin * mY[k];
-					Vec3 center = (y_i + y_j + y_k) / 3.0f;
-					Vec3 normal = (y_j - y_i).Cross(y_k - y_i);
-					if (normal.Dot(center) < 0.0f)
+					RVec3 y_k = origin * mY[k];
+					RVec3 center = (y_i + y_j + y_k) / Real(3);
+					RVec3 normal = (y_j - y_i).Cross(y_k - y_i);
+					if (normal.Dot(center) < Real(0))
 						DebugRenderer::sInstance->DrawTriangle(y_i, y_j, y_k, Color::sLightGrey);
 					else
 						DebugRenderer::sInstance->DrawTriangle(y_i, y_k, y_j, Color::sLightGrey);
@@ -963,7 +963,7 @@ private:
 
 #ifdef JPH_GJK_DEBUG
 	DebugRenderer::GeometryRef	mGeometry;	///< A visualization of the minkowski difference for state drawing
-	Vec3		mOffset = Vec3::sZero();	///< Offset to use for state drawing
+	RVec3		mOffset = RVec3::sZero();	///< Offset to use for state drawing
 #endif
 };
 

Jolt/Physics/Collision/Shape/HeightFieldShape.cpp

@@ -1309,7 +1309,7 @@ public:
 									}
 
 								#ifdef JPH_DEBUG_HEIGHT_FIELD
-									DebugRenderer::sInstance->DrawWireTriangle(v0, v1, v2, Color::sWhite);
+									DebugRenderer::sInstance->DrawWireTriangle(RVec3(v0), RVec3(v1), RVec3(v2), Color::sWhite);
 								#endif
 
 									// Call visitor

Jolt/Physics/DeterminismLog.h

@@ -25,6 +25,11 @@ private:
 		return *(uint32 *)&inValue;
 	}
 
+	JPH_INLINE uint64		Convert(double inValue) const
+	{
+		return *(uint64 *)&inValue;
+	}
+
 public:
 							DeterminismLog()
 	{
@@ -92,6 +97,12 @@ public:
 		return *this;
 	}
 	
+	DeterminismLog &		operator << (DVec3Arg inValue)
+	{
+		mLog << std::hex << std::setw(16) << Convert(inValue.GetX()) << " " << std::setw(16) << Convert(inValue.GetY()) << " " << std::setw(16) << Convert(inValue.GetZ());
+		return *this;
+	}
+	
 	DeterminismLog &		operator << (Vec4Arg inValue)
 	{
 		mLog << std::hex << std::setw(8) << Convert(inValue.GetX()) << " " << std::setw(8) << Convert(inValue.GetY()) << " " << std::setw(8) << Convert(inValue.GetZ()) << " " << std::setw(8) << Convert(inValue.GetW());
@@ -110,6 +121,12 @@ public:
 		return *this;
 	}
 
+	DeterminismLog &		operator << (DMat44Arg inValue)
+	{
+		*this << inValue.GetColumn4(0) << " " << inValue.GetColumn4(1) << " " << inValue.GetColumn4(2) << " " << inValue.GetTranslation();
+		return *this;
+	}
+
 	DeterminismLog &		operator << (QuatArg inValue)
 	{
 		*this << inValue.GetXYZW();