Supporting double precision positions to enable larger worlds (#344)

Please see the Big Worlds section in Architecture and API documentation for further information and see APIChanges.md for interface changes that come with this change.

.github/workflows/build.yml

@@ -21,12 +21,13 @@ jobs:
         matrix:
             build_type: [Debug, Release, Distribution, ReleaseASAN, ReleaseUBSAN]
             clang_version: [clang++-12, clang++-14]
+            double_precision: [No, Yes]
 
     steps:
     - name: Checkout Code
       uses: actions/checkout@v3
     - name: Configure CMake
-      run: cmake -B ${{github.workspace}}/Build/Linux_${{matrix.build_type}}_${{matrix.clang_version}} -DCMAKE_BUILD_TYPE=${{matrix.build_type}} -DCMAKE_CXX_COMPILER=${{matrix.clang_version}} Build
+      run: cmake -B ${{github.workspace}}/Build/Linux_${{matrix.build_type}}_${{matrix.clang_version}} -DCMAKE_BUILD_TYPE=${{matrix.build_type}} -DCMAKE_CXX_COMPILER=${{matrix.clang_version}} -DDOUBLE_PRECISION=${{matrix.double_precision}} Build
     - name: Build
       run: cmake --build ${{github.workspace}}/Build/Linux_${{matrix.build_type}}_${{matrix.clang_version}} -j 2
     - name: Test
@@ -88,6 +89,7 @@ jobs:
         fail-fast: false
         matrix:
             build_type: [Debug, Release, Distribution]
+            double_precision: [No, Yes]
 
     steps:
     - name: Checkout Code
@@ -95,7 +97,7 @@ jobs:
     - name: Add msbuild to PATH
       uses: microsoft/[email protected]
     - name: Configure CMake
-      run: cmake -B ${{github.workspace}}/Build/VS2022_CL -G "Visual Studio 17 2022" -A x64 Build
+      run: cmake -B ${{github.workspace}}/Build/VS2022_CL -G "Visual Studio 17 2022" -A x64 Build -DDOUBLE_PRECISION=${{matrix.double_precision}}
     - name: Build
       run: msbuild Build\VS2022_CL\JoltPhysics.sln /property:Configuration=${{matrix.build_type}}
     - name: Test

.github/workflows/determinism_check.yml

@@ -1,8 +1,8 @@
 name: Determinism Check
 
 env:
-    CONVEX_VS_MESH_HASH: '0x485e1d8e739a3c9d'
-    RAGDOLL_HASH: '0x4aa4bf5368b6027'
+    CONVEX_VS_MESH_HASH: '0xbf4b66a448ae6f68'
+    RAGDOLL_HASH: '0x588d002b30f5b3d'
 
 on:
   push:

Build/CMakeLists.txt

@@ -9,6 +9,9 @@ option(TARGET_PERFORMANCE_TEST "Build Performance Test" ON)
 option(TARGET_SAMPLES "Build Samples" ON)
 option(TARGET_VIEWER "Build JoltViewer" ON)
 
+# When turning this option on, the library will be compiled using doubles for positions. This allows for much bigger worlds.
+option(DOUBLE_PRECISION "Use double precision math" OFF)
+
 # When turning this option on, the library will be compiled with debug symbols
 option(GENERATE_DEBUG_SYMBOLS "Generate debug symbols" ON)
 
@@ -35,12 +38,18 @@ include(CMakeDependentOption)
 # Windows Store only supports the DLL version
 cmake_dependent_option(USE_STATIC_MSVC_RUNTIME_LIBRARY "Use the static MSVC runtime library" ON "MSVC;NOT WINDOWS_STORE" OFF)
 
+# Determine which configurations exist
 if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "MSVC" OR "${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU")
 	set(CMAKE_CONFIGURATION_TYPES "Debug;Release;Distribution")
 elseif ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang" OR "${CMAKE_CXX_COMPILER_ID}" STREQUAL "AppleClang")
 	set(CMAKE_CONFIGURATION_TYPES "Debug;Release;ReleaseASAN;ReleaseUBSAN;ReleaseCoverage;Distribution")
 endif()
 
+# Pass double precision flag on to the library
+if (DOUBLE_PRECISION)
+	add_compile_definitions(JPH_DOUBLE_PRECISION)
+endif()
+
 if (("${CMAKE_SYSTEM_NAME}" STREQUAL "Windows" OR "${CMAKE_SYSTEM_NAME}" STREQUAL "WindowsStore") AND NOT MINGW)
 	# Fill in the path to the asan libraries
 	set(CLANG_LIB_PATH "\"$(VSInstallDir)\\VC\\Tools\\Llvm\\x64\\lib\\clang\\${CMAKE_CXX_COMPILER_VERSION}\\lib\\windows\"")

Build/README.md

@@ -27,6 +27,7 @@ There are a number of user configurable defines that turn on/off certain feature
 - JPH_DISABLE_CUSTOM_ALLOCATOR - Disables the ability to override the memory allocator.
 - JPH_FLOATING_POINT_EXCEPTIONS_ENABLED - Turns on division by zero and invalid floating point exception support in order to detect bugs (Windows only).
 - JPH_CROSS_PLATFORM_DETERMINISTIC - Turns on behavior to attempt cross platform determinism. If this is set, JPH_USE_FMADD is ignored.
+- JPH_DOUBLE_PRECISION - Compiles the library so that all positions are stored in doubles instead of floats. This makes larger worlds possible.
 - JPH_USE_SSE4_1 - Enable SSE4.1 CPU instructions (x86/x64 only)
 - JPH_USE_SSE4_2 - Enable SSE4.2 CPU instructions (x86/x64 only)
 - JPH_USE_F16C - Enable half float CPU instructions (x86/x64 only)

Build/cmake_vs2022_cl_double.bat

@@ -0,0 +1,3 @@
+@echo off
+cmake -S . -B VS2022_CL_Double -G "Visual Studio 17 2022" -A x64 -DDOUBLE_PRECISION=ON %*
+echo Open VS2022_CL_Double\JoltPhysics.sln to build the project.

Build/cmake_vs2022_clang.bat

@@ -1,5 +1,5 @@
 @echo off
-cmake -S . -B VS2022_Clang -G "Visual Studio 17 2022" -A x64 -T ClangCL
+cmake -S . -B VS2022_Clang -G "Visual Studio 17 2022" -A x64 -T ClangCL %*
 echo:
 echo !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 echo Make sure to install:

Docs/APIChanges.md

@@ -4,6 +4,15 @@ This document lists all breaking API changes by date and by release tag. Note th
 
 Changes that make some state saved through SaveBinaryState from a prior version of the library unreadable by the new version is marked as *SBS*. See 'Saving Shapes' in [Architecture and API documentation](https://jrouwe.github.io/JoltPhysics/) for further information.
 
+## Changes between v2.0.1 and latest
+
+* TBD - Changes related to double precision support for positions (TBD)
+	* Shape::GetSubmergedVolume now takes a plane that's relative to inCenterOfMassTransform instead of one in world space
+	* Many of the NarrowPhaseQuery and TransformedShape collision queries now have a 'base offset' that you need to specify. Read the Big Worlds section in [Architecture and API documentation](https://jrouwe.github.io/JoltPhysics/) for more info.
+	* If you implement your own TempAllocator and want to compile in double precision, make sure you align to JPH_RVECTOR_ALIGNMENT bytes (instead of 16)
+	* The SkeletonPose got a 'root offset' member, this means that the ragdoll will now make the joint transform of the first body zero and put that offset in the 'root offset'.
+	* The format of a recording recorded with DebugRendererRecorder has changed, this invalidates any prior recordings.
+
 ## Changes between v1.1.0 and v2.0.0
 
 * 20221027 - *SBS* (vehicles only) - Rewrote engine model for wheeled vehicle. Before engine inertia was only used when the clutch was pressed, now it is always used, so you may want to use a lower value. The way torque is distributed over the wheels has also changed and may require tweaking the vehicle parameters. (5ac751cee9afcc097fd4f884308f5e4dc9fdaeaf)

Docs/Architecture.md

@@ -291,7 +291,23 @@ Jolt Physics uses a right handed coordinate system with Y-up. It is easy to use
 
 We use column-major vectors and matrices, this means that to transform a point you need to multiply it on the right hand side: TransformedPoint = Matrix * Point.
 
-Note that the physics simulation works best if you use SI units (meters, radians, seconds, kg). In order for the simluation to be accurate, dynamic objects should be in the order [0.1, 10] meters long and have speeds in the order of [0, 500] m/s. Static object should be in the order [0.1, 2000] meter long. If you are using different units, consider scaling the objects before passing them on to the physics simulation. Also try to ensure that all simulation takes place within 2 km from the origin to avoid floating point accuracy problems.
+Note that the physics simulation works best if you use SI units (meters, radians, seconds, kg). In order for the simluation to be accurate, dynamic objects should be in the order [0.1, 10] meters long and have speeds in the order of [0, 500] m/s. Static object should be in the order [0.1, 2000] meter long. If you are using different units, consider scaling the objects before passing them on to the physics simulation.
+
+## Big Worlds
+
+By default the library compiles using floats. This means that the simulation gets less accurate the further you go from the origin. If all simulation takes place within roughly 5 km from the origin, floating point precision is accurate enough.
+
+If you have a bigger world, you may want to compile the library using the JPH_DOUBLE_PRECISION define. When you do this, all positions will be stored as doubles, which will make the simulation accurate even at thousands of kilometers away from the origin. 
+
+Calculations with doubles are much slower than calculations with floats. A naive implementation that changes all calculations to doubles has been measured to run more than 2x slower than the same calculations using floats. Because of this, Jolt Physics will only use doubles where necessary and drop down to floats as soon as possible. In order to do this, many of the collision query functions will need a 'base offset'. All collision results will be returned as floats relative to this base offset. By choosing the base offset wisely (i.e. close to where collision results are expected) the results will be accurate. Make sure your base offset is not kilometers away from the collision result.
+
+Keep in mind that:
+
+* There are a lot of 'epsilons' in the code that have been tuned for objects of sizes/speeds as described in the previous section. Try to keep the individual objects to the specified scale even if they're really far from the origin.
+* When the collision results of a single query are kilometers apart, precision will suffer as they will be far away from the 'base offset'.
+* The effectiveness of the broad phase (which works in floats) will become less at large distances from the origin, e.g. at 10000 km from the origin, the resolution of the broad phase is reduced to 1 m which means that everything that's closer than 1 m will be considered colliding. This will not impact the quality of the simulation but it will result in extra collision tests in the narrow phase so will hurt performance.
+
+Because of the minimal use of doubles, the simulation runs 10-20% slower in double precision mode compared to float precision mode.
 
 ## Continuous Collision Detection
 

Doxyfile

@@ -2297,6 +2297,7 @@ INCLUDE_FILE_PATTERNS  =
 
 PREDEFINED             = _WIN32 \
                          JPH_PROFILE_ENABLED \
+						 JPH_DEBUG_RENDERER \
 						 JPH_PLATFORM_DOXYGEN
 
 # If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then this

HelloWorld/HelloWorld.cpp

@@ -28,6 +28,9 @@ JPH_SUPPRESS_WARNINGS
 // All Jolt symbols are in the JPH namespace
 using namespace JPH;
 
+// If you want your code to compile using single or double precision write 0.0_r to get a Real value that compiles to double or float depending if JPH_DOUBLE_PRECISION is set or not.
+using namespace JPH::literals;
+
 // We're also using STL classes in this example
 using namespace std;
 
@@ -275,7 +278,7 @@ int main(int argc, char** argv)
 	ShapeRefC floor_shape = floor_shape_result.Get(); // We don't expect an error here, but you can check floor_shape_result for HasError() / GetError()
 
 	// Create the settings for the body itself. Note that here you can also set other properties like the restitution / friction.
-	BodyCreationSettings floor_settings(floor_shape, Vec3(0.0f, -1.0f, 0.0f), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
+	BodyCreationSettings floor_settings(floor_shape, RVec3(0.0_r, -1.0_r, 0.0_r), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
 
 	// Create the actual rigid body
 	Body *floor = body_interface.CreateBody(floor_settings); // Note that if we run out of bodies this can return nullptr
@@ -285,7 +288,7 @@ int main(int argc, char** argv)
 
 	// Now create a dynamic body to bounce on the floor
 	// Note that this uses the shorthand version of creating and adding a body to the world
-	BodyCreationSettings sphere_settings(new SphereShape(0.5f), Vec3(0.0f, 2.0f, 0.0f), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
+	BodyCreationSettings sphere_settings(new SphereShape(0.5f), RVec3(0.0_r, 2.0_r, 0.0_r), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
 	BodyID sphere_id = body_interface.CreateAndAddBody(sphere_settings, EActivation::Activate);
 
 	// Now you can interact with the dynamic body, in this case we're going to give it a velocity.
@@ -308,7 +311,7 @@ int main(int argc, char** argv)
 		++step;
 
 		// Output current position and velocity of the sphere
-		Vec3 position = body_interface.GetCenterOfMassPosition(sphere_id);
+		RVec3 position = body_interface.GetCenterOfMassPosition(sphere_id);
 		Vec3 velocity = body_interface.GetLinearVelocity(sphere_id);
 		cout << "Step " << step << ": Position = (" << position.GetX() << ", " << position.GetY() << ", " << position.GetZ() << "), Velocity = (" << velocity.GetX() << ", " << velocity.GetY() << ", " << velocity.GetZ() << ")" << endl;
 

Jolt/Core/Core.h

@@ -58,6 +58,7 @@
 	#endif
 	#define JPH_USE_SSE
 	#define JPH_VECTOR_ALIGNMENT 16
+	#define JPH_DVECTOR_ALIGNMENT 32
 
 	// Detect enabled instruction sets
 	#if defined(__AVX512F__) && defined(__AVX512VL__) && defined(__AVX512DQ__) && !defined(JPH_USE_AVX512)
@@ -104,15 +105,18 @@
 		#define JPH_CPU_ADDRESS_BITS 64
 		#define JPH_USE_NEON
 		#define JPH_VECTOR_ALIGNMENT 16
+		#define JPH_DVECTOR_ALIGNMENT 32
 	#else
 		#define JPH_CPU_ADDRESS_BITS 32
 		#define JPH_VECTOR_ALIGNMENT 8 // 32-bit ARM does not support aligning on the stack on 16 byte boundaries
+		#define JPH_DVECTOR_ALIGNMENT 8
 	#endif
 #elif defined(JPH_PLATFORM_WASM)
 	// WebAssembly CPU architecture
 	#define JPH_CPU_WASM
 	#define JPH_CPU_ADDRESS_BITS 32
 	#define JPH_VECTOR_ALIGNMENT 16
+	#define JPH_DVECTOR_ALIGNMENT 32
 	#define JPH_DISABLE_CUSTOM_ALLOCATOR
 #else
 	#error Unsupported CPU architecture
@@ -352,6 +356,26 @@ static_assert(sizeof(void *) == (JPH_CPU_ADDRESS_BITS == 64? 8 : 4), "Invalid si
 	#define JPH_IF_FLOATING_POINT_EXCEPTIONS_ENABLED(...)
 #endif
 
+// Helper macros to detect if we're running in single or double precision mode
+#ifdef JPH_DOUBLE_PRECISION
+	#define JPH_IF_SINGLE_PRECISION(...)
+	#define JPH_IF_SINGLE_PRECISION_ELSE(s, d) d
+	#define JPH_IF_DOUBLE_PRECISION(...) __VA_ARGS__
+#else
+	#define JPH_IF_SINGLE_PRECISION(...) __VA_ARGS__
+	#define JPH_IF_SINGLE_PRECISION_ELSE(s, d) s
+	#define JPH_IF_DOUBLE_PRECISION(...)
+#endif
+
+// Helper macro to detect if the debug renderer is active
+#ifdef JPH_DEBUG_RENDERER
+	#define JPH_IF_DEBUG_RENDERER(...) __VA_ARGS__
+	#define JPH_IF_NOT_DEBUG_RENDERER(...)
+#else
+	#define JPH_IF_DEBUG_RENDERER(...)
+	#define JPH_IF_NOT_DEBUG_RENDERER(...) __VA_ARGS__
+#endif
+
 // Macro to indicate that a parameter / variable is unused
 #define JPH_UNUSED(x)			(void)x
 

Jolt/Core/StreamIn.h

@@ -65,6 +65,27 @@ public:
 		ReadBytes(&outVec, 3 * sizeof(float));
 		outVec = Vec3::sFixW(outVec.mValue);
 	}
+
+	/// Read a DVec3 (don't read W)
+	void				Read(DVec3 &outVec)
+	{
+		ReadBytes(&outVec, 3 * sizeof(double));
+		outVec = DVec3::sFixW(outVec.mValue);
+	}
+
+	/// Read a DMat44 (don't read W component of translation)
+	void				Read(DMat44 &outVec)
+	{
+		Vec4 x, y, z;
+		Read(x);
+		Read(y);
+		Read(z);
+
+		DVec3 t;
+		Read(t);
+
+		outVec = DMat44(x, y, z, t);
+	}
 };
 
 JPH_NAMESPACE_END

Jolt/Core/StreamOut.h

@@ -51,6 +51,22 @@ public:
 	{
 		WriteBytes(&inVec, 3 * sizeof(float));
 	}
+
+	/// Write a DVec3 (don't write W)
+	void				Write(const DVec3 &inVec)
+	{
+		WriteBytes(&inVec, 3 * sizeof(double));
+	}
+
+	/// Write a DMat44 (don't write W component of translation)
+	void				Write(const DMat44 &inVec)
+	{
+		Write(inVec.GetAxisX());
+		Write(inVec.GetAxisY());
+		Write(inVec.GetAxisZ());
+
+		Write(inVec.GetTranslation());
+	}
 };
 
 JPH_NAMESPACE_END

Jolt/Core/TempAllocator.h

@@ -19,7 +19,7 @@ public:
 	/// Destructor
 	virtual							~TempAllocator() = default;
 
-	/// Allocates inSize bytes of memory, returned memory address must be 16 byte aligned
+	/// Allocates inSize bytes of memory, returned memory address must be JPH_RVECTOR_ALIGNMENT byte aligned
 	virtual void *					Allocate(uint inSize) = 0;
 
 	/// Frees inSize bytes of memory located at inAddress
@@ -34,7 +34,7 @@ public:
 
 	/// Constructs the allocator with a maximum allocatable size of inSize
 	explicit						TempAllocatorImpl(uint inSize) :
-		mBase(static_cast<uint8 *>(JPH::Allocate(inSize))),
+		mBase(static_cast<uint8 *>(AlignedAllocate(inSize, JPH_RVECTOR_ALIGNMENT))),
 		mSize(inSize)
 	{
 	}
@@ -43,7 +43,7 @@ public:
 	virtual							~TempAllocatorImpl() override
 	{
 		JPH_ASSERT(mTop == 0);
-		JPH::Free(mBase);
+		AlignedFree(mBase);
 	}
 
 	// See: TempAllocator
@@ -55,7 +55,7 @@ public:
 		}
 		else
 		{
-			uint new_top = mTop + AlignUp(inSize, 16);
+			uint new_top = mTop + AlignUp(inSize, JPH_RVECTOR_ALIGNMENT);
 			if (new_top > mSize)
 				JPH_CRASH; // Out of memory
 			void *address = mBase + mTop;
@@ -73,7 +73,7 @@ public:
 		}
 		else
 		{
-			mTop -= AlignUp(inSize, 16);
+			mTop -= AlignUp(inSize, JPH_RVECTOR_ALIGNMENT);
 			if (mBase + mTop != inAddress)
 				JPH_CRASH; // Freeing in the wrong order
 		}
@@ -101,7 +101,7 @@ public:
 	// See: TempAllocator
 	virtual void *					Allocate(uint inSize) override
 	{
-		return AlignedAllocate(inSize, 16);
+		return AlignedAllocate(inSize, JPH_RVECTOR_ALIGNMENT);
 	}
 
 	// See: TempAllocator

Jolt/Geometry/AABox.h

@@ -19,6 +19,7 @@ public:
 	/// Constructor
 					AABox()												: mMin(Vec3::sReplicate(FLT_MAX)), mMax(Vec3::sReplicate(-FLT_MAX)) { }
 					AABox(Vec3Arg inMin, Vec3Arg inMax)					: mMin(inMin), mMax(inMax) { }
+					AABox(DVec3Arg inMin, DVec3Arg inMax)				: mMin(inMin.ToVec3RoundDown()), mMax(inMax.ToVec3RoundUp()) { }
 					AABox(Vec3Arg inCenter, float inRadius)				: mMin(inCenter - Vec3::sReplicate(inRadius)), mMax(inCenter + Vec3::sReplicate(inRadius)) { }
 
 	/// Create box from 2 points
@@ -143,6 +144,12 @@ public:
 		return UVec4::sAnd(Vec3::sLessOrEqual(mMin, inOther), Vec3::sGreaterOrEqual(mMax, inOther)).TestAllXYZTrue();
 	}
 
+	/// Check if this box contains a point
+	bool			Contains(DVec3Arg inOther) const
+	{
+		return Contains(Vec3(inOther));
+	}
+
 	/// Check if this box overlaps with another box
 	bool			Overlaps(const AABox &inOther) const
 	{
@@ -165,6 +172,13 @@ public:
 		mMax += inTranslation;
 	}
 
+	/// Translate bounding box
+	void			Translate(DVec3Arg inTranslation)
+	{
+		mMin = (DVec3(mMin) + inTranslation).ToVec3RoundDown();
+		mMax = (DVec3(mMax) + inTranslation).ToVec3RoundUp();
+	}
+
 	/// Transform bounding box
 	AABox			Transformed(Mat44Arg inMatrix) const
 	{
@@ -188,6 +202,14 @@ public:
 		return AABox(new_min, new_max);
 	}
 
+	/// Transform bounding box
+	AABox			Transformed(DMat44Arg inMatrix) const
+	{
+		AABox transformed = Transformed(inMatrix.GetRotation());
+		transformed.Translate(inMatrix.GetTranslation());
+		return transformed;
+	}
+
 	/// Scale this bounding box, can handle non-uniform and negative scaling
 	AABox			Scaled(Vec3Arg inScale) const
 	{

Jolt/Geometry/Plane.h

@@ -19,6 +19,9 @@ public:
 	/// Create from point and normal
 	static Plane	sFromPointAndNormal(Vec3Arg inPoint, Vec3Arg inNormal)					{ return Plane(Vec4(inNormal, -inNormal.Dot(inPoint))); }
 
+	/// Create from point and normal, double precision version that more accurately calculates the plane constant
+	static Plane	sFromPointAndNormal(DVec3Arg inPoint, Vec3Arg inNormal)					{ return Plane(Vec4(inNormal, -float(DVec3(inNormal).Dot(inPoint)))); }
+
 	/// Create from 3 counter clockwise points
 	static Plane	sFromPointsCCW(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3)				{ return sFromPointAndNormal(inV1, (inV2 - inV1).Cross(inV3 - inV1).Normalized()); }
 

Jolt/Jolt.cmake

@@ -91,6 +91,9 @@ set(JOLT_PHYSICS_SRC_FILES
 	${JOLT_PHYSICS_ROOT}/Geometry/Triangle.h
 	${JOLT_PHYSICS_ROOT}/Jolt.cmake
 	${JOLT_PHYSICS_ROOT}/Jolt.h
+	${JOLT_PHYSICS_ROOT}/Math/DMat44.h
+	${JOLT_PHYSICS_ROOT}/Math/DMat44.inl
+	${JOLT_PHYSICS_ROOT}/Math/Double3.h
 	${JOLT_PHYSICS_ROOT}/Math/DVec3.h
 	${JOLT_PHYSICS_ROOT}/Math/DVec3.inl
 	${JOLT_PHYSICS_ROOT}/Math/DynMatrix.h
@@ -108,6 +111,7 @@ set(JOLT_PHYSICS_SRC_FILES
 	${JOLT_PHYSICS_ROOT}/Math/Matrix.h
 	${JOLT_PHYSICS_ROOT}/Math/Quat.h
 	${JOLT_PHYSICS_ROOT}/Math/Quat.inl
+	${JOLT_PHYSICS_ROOT}/Math/Real.h
 	${JOLT_PHYSICS_ROOT}/Math/Swizzle.h
 	${JOLT_PHYSICS_ROOT}/Math/Trigonometry.h
 	${JOLT_PHYSICS_ROOT}/Math/UVec4.cpp

Jolt/Jolt.h

@@ -12,3 +12,4 @@
 #include <Jolt/Math/Math.h>
 #include <Jolt/Math/Vec4.h>
 #include <Jolt/Math/Mat44.h>
+#include <Jolt/Math/Real.h>

Jolt/Jolt.natvis

@@ -13,7 +13,10 @@
     <DisplayString>{x}, {y}, {z}, {w}</DisplayString>
   </Type>
   <Type Name="JPH::Vec3">
-  <DisplayString>{mF32[0]}, {mF32[1]}, {mF32[2]}, L^2={mF32[0]*mF32[0]+mF32[1]*mF32[1]+mF32[2]*mF32[2]}</DisplayString>
+    <DisplayString>{mF32[0]}, {mF32[1]}, {mF32[2]}, L^2={mF32[0]*mF32[0]+mF32[1]*mF32[1]+mF32[2]*mF32[2]}</DisplayString>
+  </Type>
+  <Type Name="JPH::DVec3">
+    <DisplayString>{mF64[0]}, {mF64[1]}, {mF64[2]}, L^2={mF64[0]*mF64[0]+mF64[1]*mF64[1]+mF64[2]*mF64[2]}</DisplayString>
   </Type>
   <Type Name="JPH::Vec4">
     <DisplayString>{mF32[0]}, {mF32[1]}, {mF32[2]}, {mF32[3]}, L^2={mF32[0]*mF32[0]+mF32[1]*mF32[1]+mF32[2]*mF32[2]+mF32[3]*mF32[3]}</DisplayString>
@@ -41,6 +44,23 @@
       </Synthetic>
     </Expand>
   </Type>
+  <Type Name="JPH::DMat44">
+    <DisplayString>{mCol[0].mF32[0]}, {mCol[1].mF32[0]}, {mCol[2].mF32[0]}, {mCol3.mF64[0]} | {mCol[0].mF32[1]}, {mCol[1].mF32[1]}, {mCol[2].mF32[1]}, {mCol3.mF64[1]} | {mCol[0].mF32[2]}, {mCol[1].mF32[2]}, {mCol[2].mF32[2]}, {mCol3.mF64[2]}</DisplayString>
+    <Expand>
+      <Synthetic Name="[Row 0]">
+        <DisplayString>{mCol[0].mF32[0]}, {mCol[1].mF32[0]}, {mCol[2].mF32[0]}, {mCol3.mF64[0]}</DisplayString>
+      </Synthetic>
+      <Synthetic Name="[Row 1]">
+        <DisplayString>{mCol[0].mF32[1]}, {mCol[1].mF32[1]}, {mCol[2].mF32[1]}, {mCol3.mF64[1]}</DisplayString>
+      </Synthetic>
+      <Synthetic Name="[Row 2]">
+        <DisplayString>{mCol[0].mF32[2]}, {mCol[1].mF32[2]}, {mCol[2].mF32[2]}, {mCol3.mF64[2]}</DisplayString>
+      </Synthetic>
+      <Synthetic Name="[Row 3]">
+        <DisplayString>{mCol[0].mF32[3]}, {mCol[1].mF32[3]}, {mCol[2].mF32[3]}, 1}</DisplayString>
+      </Synthetic>
+    </Expand>
+  </Type>
   <Type Name="JPH::AABox">
     <DisplayString>min=({mMin}), max=({mMax})</DisplayString>
   </Type>

Jolt/Math/DMat44.h

@@ -0,0 +1,153 @@
+// SPDX-FileCopyrightText: 2022 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Math/MathTypes.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Holds a 4x4 matrix of floats with the last column consisting of doubles
+class [[nodiscard]] alignas(JPH_DVECTOR_ALIGNMENT) DMat44
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+	// Underlying column type
+	using Type = Vec4::Type;
+	using DType = DVec3::Type;
+	using DTypeArg = DVec3::TypeArg;
+
+	// Argument type
+	using ArgType = DMat44Arg;
+
+	/// Constructor
+								DMat44() = default; ///< Intentionally not initialized for performance reasons
+	JPH_INLINE					DMat44(Vec4Arg inC1, Vec4Arg inC2, Vec4Arg inC3, DVec3Arg inC4);
+								DMat44(const DMat44 &inM2) = default;
+	JPH_INLINE explicit			DMat44(Mat44Arg inM);
+	JPH_INLINE					DMat44(Mat44Arg inRot, DVec3Arg inT);
+	JPH_INLINE					DMat44(Type inC1, Type inC2, Type inC3, DTypeArg inC4);
+
+	/// Zero matrix
+	static JPH_INLINE DMat44	sZero();
+
+	/// Identity matrix
+	static JPH_INLINE DMat44	sIdentity();
+
+	/// Rotate from quaternion
+	static JPH_INLINE DMat44	sRotation(QuatArg inQuat)								{ return DMat44(Mat44::sRotation(inQuat), DVec3::sZero()); }
+
+	/// Get matrix that translates
+	static JPH_INLINE DMat44	sTranslation(DVec3Arg inV)								{ return DMat44(Vec4(1, 0, 0, 0), Vec4(0, 1, 0, 0), Vec4(0, 0, 1, 0), inV); }
+
+	/// Get matrix that rotates and translates
+	static JPH_INLINE DMat44	sRotationTranslation(QuatArg inR, DVec3Arg inT)			{ return DMat44(Mat44::sRotation(inR), inT); }
+
+	/// Get inverse matrix of sRotationTranslation
+	static JPH_INLINE DMat44	sInverseRotationTranslation(QuatArg inR, DVec3Arg inT);
+
+	/// Get matrix that scales (produces a matrix with (inV, 1) on its diagonal)
+	static JPH_INLINE DMat44	sScale(Vec3Arg inV)										{ return DMat44(Mat44::sScale(inV), DVec3::sZero()); }
+
+	/// Convert to Mat44 rounding to nearest
+	JPH_INLINE Mat44			ToMat44() const											{ return Mat44(mCol[0], mCol[1], mCol[2], Vec3(mCol3)); }
+
+	/// Comparsion
+	JPH_INLINE bool				operator == (DMat44Arg inM2) const;
+	JPH_INLINE bool				operator != (DMat44Arg inM2) const						{ return !(*this == inM2); }
+
+	/// Test if two matrices are close
+	JPH_INLINE bool				IsClose(DMat44Arg inM2, float inMaxDistSq = 1.0e-12f) const;
+
+	/// Multiply matrix by matrix
+	JPH_INLINE DMat44			operator * (Mat44Arg inM) const;
+
+	/// Multiply matrix by matrix
+	JPH_INLINE DMat44			operator * (DMat44Arg inM) const;
+
+	/// Multiply vector by matrix
+	JPH_INLINE DVec3			operator * (Vec3Arg inV) const;
+
+	/// Multiply vector by matrix
+	JPH_INLINE DVec3			operator * (DVec3Arg inV) const;
+
+	/// Multiply vector by only 3x3 part of the matrix
+	JPH_INLINE Vec3				Multiply3x3(Vec3Arg inV) const							{ return GetRotation().Multiply3x3(inV); }
+
+	/// Multiply vector by only 3x3 part of the matrix
+	JPH_INLINE DVec3			Multiply3x3(DVec3Arg inV) const;
+
+	/// Multiply vector by only 3x3 part of the transpose of the matrix (\f$result = this^T \: inV\f$)
+	JPH_INLINE Vec3				Multiply3x3Transposed(Vec3Arg inV) const				{ return GetRotation().Multiply3x3Transposed(inV); }
+
+	/// Scale a matrix: result = this * Mat44::sScale(inScale)
+	JPH_INLINE DMat44			PreScaled(Vec3Arg inScale) const;
+
+	/// Scale a matrix: result = Mat44::sScale(inScale) * this
+	JPH_INLINE DMat44			PostScaled(Vec3Arg inScale) const;
+
+	/// Pre multiply by translation matrix: result = this * Mat44::sTranslation(inTranslation)
+	JPH_INLINE DMat44			PreTranslated(Vec3Arg inTranslation) const;
+
+	/// Pre multiply by translation matrix: result = this * Mat44::sTranslation(inTranslation)
+	JPH_INLINE DMat44			PreTranslated(DVec3Arg inTranslation) const;
+
+	/// Post multiply by translation matrix: result = Mat44::sTranslation(inTranslation) * this (i.e. add inTranslation to the 4-th column)
+	JPH_INLINE DMat44			PostTranslated(Vec3Arg inTranslation) const;
+
+	/// Post multiply by translation matrix: result = Mat44::sTranslation(inTranslation) * this (i.e. add inTranslation to the 4-th column)
+	JPH_INLINE DMat44			PostTranslated(DVec3Arg inTranslation) const;
+
+	/// Access to the columns
+	JPH_INLINE Vec3				GetAxisX() const										{ return Vec3(mCol[0]); }
+	JPH_INLINE void				SetAxisX(Vec3Arg inV)									{ mCol[0] = Vec4(inV, 0.0f); }
+	JPH_INLINE Vec3				GetAxisY() const										{ return Vec3(mCol[1]); }
+	JPH_INLINE void				SetAxisY(Vec3Arg inV)									{ mCol[1] = Vec4(inV, 0.0f); }
+	JPH_INLINE Vec3				GetAxisZ() const										{ return Vec3(mCol[2]); }
+	JPH_INLINE void				SetAxisZ(Vec3Arg inV)									{ mCol[2] = Vec4(inV, 0.0f); }
+	JPH_INLINE DVec3			GetTranslation() const									{ return mCol3; }
+	JPH_INLINE void				SetTranslation(DVec3Arg inV)							{ mCol3 = inV; }
+	JPH_INLINE Vec3				GetColumn3(uint inCol) const							{ JPH_ASSERT(inCol < 3); return Vec3(mCol[inCol]); }
+	JPH_INLINE void				SetColumn3(uint inCol, Vec3Arg inV)						{ JPH_ASSERT(inCol < 3); mCol[inCol] = Vec4(inV, 0.0f); }
+	JPH_INLINE Vec4				GetColumn4(uint inCol) const							{ JPH_ASSERT(inCol < 3); return mCol[inCol]; }
+	JPH_INLINE void				SetColumn4(uint inCol, Vec4Arg inV)						{ JPH_ASSERT(inCol < 3); mCol[inCol] = inV; }
+
+	/// Inverse 4x4 matrix
+	JPH_INLINE DMat44			Inversed() const;
+
+	/// Inverse 4x4 matrix when it only contains rotation and translation
+	JPH_INLINE DMat44			InversedRotationTranslation() const;
+
+	/// Get rotation part only (note: retains the first 3 values from the bottom row)
+	JPH_INLINE Mat44			GetRotation() const										{ return Mat44(mCol[0], mCol[1], mCol[2], Vec4(0, 0, 0, 1)); }
+
+	/// Updates the rotation part of this matrix (the first 3 columns)
+	JPH_INLINE void				SetRotation(Mat44Arg inRotation);
+
+	/// Convert to quaternion
+	JPH_INLINE Quat				GetQuaternion() const									{ return GetRotation().GetQuaternion(); }
+
+	/// Get matrix that transforms a direction with the same transform as this matrix (length is not preserved)
+	JPH_INLINE Mat44			GetDirectionPreservingMatrix() const					{ return GetRotation().Inversed3x3().Transposed3x3(); }
+
+	/// Works identical to Mat44::Decompose
+	JPH_INLINE DMat44			Decompose(Vec3 &outScale) const							{ return DMat44(GetRotation().Decompose(outScale), mCol3); }
+
+	/// To String
+	friend ostream &			operator << (ostream &inStream, DMat44Arg inM)
+	{
+		inStream << inM.mCol[0] << ", " << inM.mCol[1] << ", " << inM.mCol[2] << ", " << inM.mCol3;
+		return inStream;
+	}
+
+private:
+	Vec4						mCol[3];												///< Rotation columns
+	DVec3						mCol3;													///< Translation column, 4th element is assumed to be 1
+};
+
+static_assert(is_trivial<DMat44>(), "Is supposed to be a trivial type!");
+
+JPH_NAMESPACE_END
+
+#include "DMat44.inl"

Jolt/Math/DMat44.inl

@@ -0,0 +1,238 @@
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Math/DVec3.h>
+
+JPH_NAMESPACE_BEGIN
+
+DMat44::DMat44(Vec4Arg inC1, Vec4Arg inC2, Vec4Arg inC3, DVec3Arg inC4) : 
+	mCol { inC1, inC2, inC3 },
+	mCol3(inC4)
+{ 
+}
+
+DMat44::DMat44(Type inC1, Type inC2, Type inC3, DTypeArg inC4) : 
+	mCol { inC1, inC2, inC3 },
+	mCol3(inC4)
+{
+}
+
+DMat44::DMat44(Mat44Arg inM) :
+	mCol { inM.GetColumn4(0), inM.GetColumn4(1), inM.GetColumn4(2) },
+	mCol3(inM.GetTranslation())
+{
+}
+
+DMat44::DMat44(Mat44Arg inRot, DVec3Arg inT) :
+	mCol { inRot.GetColumn4(0), inRot.GetColumn4(1), inRot.GetColumn4(2) },
+	mCol3(inT)
+{
+}
+
+DMat44 DMat44::sZero()
+{
+	return DMat44(Vec4::sZero(), Vec4::sZero(), Vec4::sZero(), DVec3::sZero());
+}
+
+DMat44 DMat44::sIdentity()
+{
+	return DMat44(Vec4(1, 0, 0, 0), Vec4(0, 1, 0, 0), Vec4(0, 0, 1, 0), DVec3::sZero());
+}
+
+DMat44 DMat44::sInverseRotationTranslation(QuatArg inR, DVec3Arg inT)
+{
+	Mat44 m = Mat44::sRotation(inR.Conjugated());
+	DMat44 dm(m, DVec3::sZero());
+	dm.SetTranslation(-dm.Multiply3x3(inT));
+	return dm;
+}
+
+bool DMat44::operator == (DMat44Arg inM2) const
+{
+	return mCol[0] == inM2.mCol[0]
+		&& mCol[1] == inM2.mCol[1]
+		&& mCol[2] == inM2.mCol[2]
+		&& mCol3 == inM2.mCol3;
+}
+
+bool DMat44::IsClose(DMat44Arg inM2, float inMaxDistSq) const
+{
+	for (int i = 0; i < 3; ++i)
+		if (!mCol[i].IsClose(inM2.mCol[i], inMaxDistSq))
+			return false;
+	return mCol3.IsClose(inM2.mCol3, double(inMaxDistSq));
+}
+
+DMat44 DMat44::operator * (Mat44Arg inM) const
+{
+	DMat44 result;
+
+	// Rotation part
+#if defined(JPH_USE_SSE)
+	for (int i = 0; i < 3; ++i)
+	{
+		__m128 c = inM.GetColumn4(i).mValue;
+		__m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(0, 0, 0, 0)));
+		t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(1, 1, 1, 1))));
+		t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(2, 2, 2, 2))));
+		result.mCol[i].mValue = t;
+	}
+#else
+	for (int i = 0; i < 3; ++i)
+	{
+		Vec4 coli = inM.GetColumn4(i);
+		result.mCol[i] = mCol[0] * coli.mF32[0] + mCol[1] * coli.mF32[1] + mCol[2] * coli.mF32[2];
+	}
+#endif
+
+	// Translation part
+#if defined(JPH_USE_AVX)
+	__m128 col3 = inM.GetColumn4(3).mValue;
+	__m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(col3, col3, _MM_SHUFFLE(0, 0, 0, 0)));
+	t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(col3, col3, _MM_SHUFFLE(1, 1, 1, 1))));
+	t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(col3, col3, _MM_SHUFFLE(2, 2, 2, 2))));
+	result.mCol3 = DVec3::sFixW(_mm256_add_pd(mCol3.mValue, _mm256_cvtps_pd(t)));
+#else
+	Vec4 col3 = inM.GetColumn4(3);
+	result.mCol3 = mCol3 + Vec3(mCol[0] * col3.mF32[0] + mCol[1] * col3.mF32[1] + mCol[2] * col3.mF32[2]);
+#endif
+
+	return result;
+}
+
+DMat44 DMat44::operator * (DMat44Arg inM) const
+{
+	DMat44 result;
+
+	// Rotation part
+#if defined(JPH_USE_SSE)
+	for (int i = 0; i < 3; ++i)
+	{
+		__m128 c = inM.mCol[i].mValue;
+		__m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(0, 0, 0, 0)));
+		t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(1, 1, 1, 1))));
+		t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(2, 2, 2, 2))));
+		result.mCol[i].mValue = t;
+	}
+#else
+	for (int i = 0; i < 3; ++i)
+	{
+		Vec4 coli = inM.mCol[i];
+		result.mCol[i] = mCol[0] * coli.mF32[0] + mCol[1] * coli.mF32[1] + mCol[2] * coli.mF32[2];
+	}
+#endif // JPH_USE_SSE
+
+	// Translation part
+#if defined(JPH_USE_AVX)
+	__m256d t = mCol3.mValue;
+	t = _mm256_add_pd(t, _mm256_mul_pd(_mm256_cvtps_pd(mCol[0].mValue), _mm256_set1_pd(inM.mCol3.mF64[0])));
+	t = _mm256_add_pd(t, _mm256_mul_pd(_mm256_cvtps_pd(mCol[1].mValue), _mm256_set1_pd(inM.mCol3.mF64[1])));
+	t = _mm256_add_pd(t, _mm256_mul_pd(_mm256_cvtps_pd(mCol[2].mValue), _mm256_set1_pd(inM.mCol3.mF64[2])));
+	result.mCol3 = DVec3::sFixW(t);
+#else
+	result.mCol3 = mCol3 + DVec3(mCol[0]) * inM.mCol3.mF64[0] + DVec3(mCol[1]) * inM.mCol3.mF64[1] + DVec3(mCol[2]) * inM.mCol3.mF64[2];
+#endif
+
+	return result;
+}
+
+DVec3 DMat44::operator * (Vec3Arg inV) const
+{
+#if defined(JPH_USE_AVX)
+	__m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(0, 0, 0, 0)));
+	t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(1, 1, 1, 1))));
+	t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(2, 2, 2, 2))));
+	return DVec3::sFixW(_mm256_add_pd(mCol3.mValue, _mm256_cvtps_pd(t)));
+#else
+	return DVec3(
+		mCol3.mF64[0] + double(mCol[0].mF32[0] * inV.mF32[0] + mCol[1].mF32[0] * inV.mF32[1] + mCol[2].mF32[0] * inV.mF32[2]), 
+		mCol3.mF64[1] + double(mCol[0].mF32[1] * inV.mF32[0] + mCol[1].mF32[1] * inV.mF32[1] + mCol[2].mF32[1] * inV.mF32[2]), 
+		mCol3.mF64[2] + double(mCol[0].mF32[2] * inV.mF32[0] + mCol[1].mF32[2] * inV.mF32[1] + mCol[2].mF32[2] * inV.mF32[2]));
+#endif
+}
+
+DVec3 DMat44::operator * (DVec3Arg inV) const
+{
+#if defined(JPH_USE_AVX)
+	__m256d t = _mm256_add_pd(mCol3.mValue, _mm256_mul_pd(_mm256_cvtps_pd(mCol[0].mValue), _mm256_set1_pd(inV.mF64[0])));
+	t = _mm256_add_pd(t, _mm256_mul_pd(_mm256_cvtps_pd(mCol[1].mValue), _mm256_set1_pd(inV.mF64[1])));
+	t = _mm256_add_pd(t, _mm256_mul_pd(_mm256_cvtps_pd(mCol[2].mValue), _mm256_set1_pd(inV.mF64[2])));
+	return DVec3::sFixW(t);
+#else
+	return DVec3(
+		mCol3.mF64[0] + double(mCol[0].mF32[0]) * inV.mF64[0] + double(mCol[1].mF32[0]) * inV.mF64[1] + double(mCol[2].mF32[0]) * inV.mF64[2], 
+		mCol3.mF64[1] + double(mCol[0].mF32[1]) * inV.mF64[0] + double(mCol[1].mF32[1]) * inV.mF64[1] + double(mCol[2].mF32[1]) * inV.mF64[2], 
+		mCol3.mF64[2] + double(mCol[0].mF32[2]) * inV.mF64[0] + double(mCol[1].mF32[2]) * inV.mF64[1] + double(mCol[2].mF32[2]) * inV.mF64[2]);
+#endif
+}
+
+DVec3 DMat44::Multiply3x3(DVec3Arg inV) const
+{
+#if defined(JPH_USE_AVX)
+	__m256d t = _mm256_mul_pd(_mm256_cvtps_pd(mCol[0].mValue), _mm256_set1_pd(inV.mF64[0]));
+	t = _mm256_add_pd(t, _mm256_mul_pd(_mm256_cvtps_pd(mCol[1].mValue), _mm256_set1_pd(inV.mF64[1])));
+	t = _mm256_add_pd(t, _mm256_mul_pd(_mm256_cvtps_pd(mCol[2].mValue), _mm256_set1_pd(inV.mF64[2])));
+	return DVec3::sFixW(t);
+#else
+	return DVec3(
+		double(mCol[0].mF32[0]) * inV.mF64[0] + double(mCol[1].mF32[0]) * inV.mF64[1] + double(mCol[2].mF32[0]) * inV.mF64[2], 
+		double(mCol[0].mF32[1]) * inV.mF64[0] + double(mCol[1].mF32[1]) * inV.mF64[1] + double(mCol[2].mF32[1]) * inV.mF64[2], 
+		double(mCol[0].mF32[2]) * inV.mF64[0] + double(mCol[1].mF32[2]) * inV.mF64[1] + double(mCol[2].mF32[2]) * inV.mF64[2]);
+#endif
+}
+
+void DMat44::SetRotation(Mat44Arg inRotation)
+{
+	mCol[0] = inRotation.GetColumn4(0);
+	mCol[1] = inRotation.GetColumn4(1);
+	mCol[2] = inRotation.GetColumn4(2);
+}
+
+DMat44 DMat44::PreScaled(Vec3Arg inScale) const
+{
+	return DMat44(inScale.GetX() * mCol[0], inScale.GetY() * mCol[1], inScale.GetZ() * mCol[2], mCol3);
+}
+
+DMat44 DMat44::PostScaled(Vec3Arg inScale) const
+{
+	Vec4 scale(inScale, 1);
+	return DMat44(scale * mCol[0], scale * mCol[1], scale * mCol[2], DVec3(scale) * mCol3);
+}
+
+DMat44 DMat44::PreTranslated(Vec3Arg inTranslation) const
+{
+	return DMat44(mCol[0], mCol[1], mCol[2], GetTranslation() + Multiply3x3(inTranslation));
+}
+
+DMat44 DMat44::PreTranslated(DVec3Arg inTranslation) const
+{
+	return DMat44(mCol[0], mCol[1], mCol[2], GetTranslation() + Multiply3x3(inTranslation));
+}
+
+DMat44 DMat44::PostTranslated(Vec3Arg inTranslation) const
+{
+	return DMat44(mCol[0], mCol[1], mCol[2], GetTranslation() + inTranslation);
+}
+
+DMat44 DMat44::PostTranslated(DVec3Arg inTranslation) const
+{
+	return DMat44(mCol[0], mCol[1], mCol[2], GetTranslation() + inTranslation);
+}
+
+DMat44 DMat44::Inversed() const
+{
+	DMat44 m(GetRotation().Inversed3x3());
+	m.mCol3 = -m.Multiply3x3(mCol3);
+	return m;
+}
+
+DMat44 DMat44::InversedRotationTranslation() const
+{
+	DMat44 m(GetRotation().Transposed3x3());
+	m.mCol3 = -m.Multiply3x3(mCol3);
+	return m;
+}
+
+JPH_NAMESPACE_END

Jolt/Math/DVec3.h

@@ -3,13 +3,13 @@
 
 #pragma once
 
-#include <Jolt/Math/Swizzle.h>
+#include <Jolt/Math/Double3.h>
 
 JPH_NAMESPACE_BEGIN
 
 /// 3 component vector of doubles (stored as 4 vectors). 
 /// Note that we keep the 4th component the same as the 3rd component to avoid divisions by zero when JPH_FLOATING_POINT_EXCEPTIONS_ENABLED defined
-class [[nodiscard]] DVec3
+class [[nodiscard]] alignas(JPH_DVECTOR_ALIGNMENT) DVec3
 {
 public:
 	JPH_OVERRIDE_NEW_DELETE
@@ -17,21 +17,27 @@ public:
 	// Underlying vector type
 #if defined(JPH_USE_AVX)
 	using Type = __m256d;
+	using TypeArg = __m256d;
 #else
 	using Type = struct { double mData[4]; };
+	using TypeArg = const Type &;
 #endif
 
+	// Argument type
+	using ArgType = DVec3Arg;
+
 	/// Constructor
 								DVec3() = default; ///< Intentionally not initialized for performance reasons
 								DVec3(const DVec3 &inRHS) = default;
 	JPH_INLINE explicit			DVec3(Vec3Arg inRHS);
-	JPH_INLINE					DVec3(Type inRHS) : mValue(inRHS)				{ CheckW(); }
+	JPH_INLINE explicit			DVec3(Vec4Arg inRHS);
+	JPH_INLINE					DVec3(TypeArg inRHS) : mValue(inRHS)			{ CheckW(); }
 
 	/// Create a vector from 3 components
 	JPH_INLINE					DVec3(double inX, double inY, double inZ);
 
 	/// Load 3 doubles from memory
-	explicit JPH_INLINE			DVec3(const double *inV);
+	explicit JPH_INLINE			DVec3(const Double3 &inV);
 
 	/// Vector with all zeros
 	static JPH_INLINE DVec3		sZero();
@@ -44,11 +50,29 @@ public:
 	/// Replicate inV across all components
 	static JPH_INLINE DVec3		sReplicate(double inV);
 		
+	/// Vector with all NaN's
+	static JPH_INLINE DVec3		sNaN();
+
 	/// Load 3 doubles from memory (reads 64 bits extra which it doesn't use)
-	static JPH_INLINE DVec3		sLoadDouble3Unsafe(const double *inV);
+	static JPH_INLINE DVec3		sLoadDouble3Unsafe(const Double3 &inV);
+
+	/// Store 3 doubles to memory
+	JPH_INLINE void				StoreDouble3(Double3 *outV) const;
+
+	/// Convert to float vector 3 rounding to nearest
+	JPH_INLINE explicit			operator Vec3() const;
+
+	/// Prepare to convert to float vector 3 rounding towards zero (returns DVec3 that can be converted to a Vec3 to get the rounding)
+	JPH_INLINE DVec3			PrepareRoundToZero() const;
+
+	/// Prepare to convert to float vector 3 rounding towards positive/negative inf  (returns DVec3 that can be converted to a Vec3 to get the rounding)
+	JPH_INLINE DVec3			PrepareRoundToInf() const;
 
-	/// Convert to float vector 3
-	JPH_INLINE Vec3				ToVec3() const;
+	/// Convert to float vector 3 rounding down
+	JPH_INLINE Vec3				ToVec3RoundDown() const;
+
+	/// Convert to float vector 3 rounding up
+	JPH_INLINE Vec3				ToVec3RoundUp() const;
 
 	/// Return the minimum value of each of the components
 	static JPH_INLINE DVec3		sMin(DVec3Arg inV1, DVec3Arg inV2);
@@ -102,21 +126,21 @@ public:
 #ifdef JPH_USE_AVX
 	JPH_INLINE double			GetX() const									{ return _mm_cvtsd_f64(_mm256_castpd256_pd128(mValue)); }
 #else
-	JPH_INLINE double			GetX() const									{ return mD32[0]; }
+	JPH_INLINE double			GetX() const									{ return mF64[0]; }
 #endif // JPH_USE_AVX
-	JPH_INLINE double			GetY() const									{ return mD32[1]; }
-	JPH_INLINE double			GetZ() const									{ return mD32[2]; }
+	JPH_INLINE double			GetY() const									{ return mF64[1]; }
+	JPH_INLINE double			GetZ() const									{ return mF64[2]; }
 	
 	/// Set individual components
-	JPH_INLINE void				SetX(double inX)								{ mD32[0] = inX; }
-	JPH_INLINE void				SetY(double inY)								{ mD32[1] = inY; }
-	JPH_INLINE void				SetZ(double inZ)								{ mD32[2] = mD32[3] = inZ; } // Assure Z and W are the same
+	JPH_INLINE void				SetX(double inX)								{ mF64[0] = inX; }
+	JPH_INLINE void				SetY(double inY)								{ mF64[1] = inY; }
+	JPH_INLINE void				SetZ(double inZ)								{ mF64[2] = mF64[3] = inZ; } // Assure Z and W are the same
 	
 	/// Get double component by index
-	JPH_INLINE double			operator [] (uint inCoordinate) const			{ JPH_ASSERT(inCoordinate < 3); return mD32[inCoordinate]; }
+	JPH_INLINE double			operator [] (uint inCoordinate) const			{ JPH_ASSERT(inCoordinate < 3); return mF64[inCoordinate]; }
 
 	/// Set double component by index
-	JPH_INLINE void				SetComponent(uint inCoordinate, double inValue)	{ JPH_ASSERT(inCoordinate < 3); mD32[inCoordinate] = inValue; mValue = sFixW(mValue); } // Assure Z and W are the same
+	JPH_INLINE void				SetComponent(uint inCoordinate, double inValue)	{ JPH_ASSERT(inCoordinate < 3); mF64[inCoordinate] = inValue; mValue = sFixW(mValue); } // Assure Z and W are the same
 
 	/// Comparison
 	JPH_INLINE bool				operator == (DVec3Arg inV2) const;
@@ -131,6 +155,9 @@ public:
 	/// Test if vector is normalized
 	JPH_INLINE bool				IsNormalized(double inTolerance = 1.0e-12) const;
 
+	/// Test if vector contains NaN elements
+	JPH_INLINE bool				IsNaN() const;
+
 	/// Multiply two double vectors (component wise)
 	JPH_INLINE DVec3			operator * (DVec3Arg inV2) const;
 
@@ -152,18 +179,30 @@ public:
 	/// Divide vector by double
 	JPH_INLINE DVec3 &			operator /= (double inV2);
 
+	/// Add two vectors (component wise)
+	JPH_INLINE DVec3			operator + (Vec3Arg inV2) const;
+
 	/// Add two double vectors (component wise)
 	JPH_INLINE DVec3			operator + (DVec3Arg inV2) const;
 
+	/// Add two vectors (component wise)
+	JPH_INLINE DVec3 &			operator += (Vec3Arg inV2);
+
 	/// Add two double vectors (component wise)
 	JPH_INLINE DVec3 &			operator += (DVec3Arg inV2);
 
 	/// Negate
 	JPH_INLINE DVec3			operator - () const;
 
+	/// Subtract two vectors (component wise)
+	JPH_INLINE DVec3			operator - (Vec3Arg inV2) const;
+
 	/// Subtract two double vectors (component wise)
 	JPH_INLINE DVec3			operator - (DVec3Arg inV2) const;
 
+	/// Add two vectors (component wise)
+	JPH_INLINE DVec3 &			operator -= (Vec3Arg inV2);
+
 	/// Add two double vectors (component wise)
 	JPH_INLINE DVec3 &			operator -= (DVec3Arg inV2);
 
@@ -200,7 +239,7 @@ public:
 	/// To String
 	friend ostream &			operator << (ostream &inStream, DVec3Arg inV)
 	{
-		inStream << inV.mD32[0] << ", " << inV.mD32[1] << ", " << inV.mD32[2];
+		inStream << inV.mF64[0] << ", " << inV.mF64[1] << ", " << inV.mF64[2];
 		return inStream;
 	}
 
@@ -208,17 +247,16 @@ public:
 	JPH_INLINE void				CheckW() const;
 	
 	/// Internal helper function that ensures that the Z component is replicated to the W component to prevent divisions by zero
-	static JPH_INLINE Type		sFixW(Type inValue);
+	static JPH_INLINE Type		sFixW(TypeArg inValue);
 
 	/// Representations of true and false for boolean operations
 	inline static const double	cTrue = BitCast<double>(~uint64(0));
-	inline static const double	cFalse = 0.0f;
+	inline static const double	cFalse = 0.0;
 
-private:
 	union
 	{
 		Type					mValue;
-		double					mD32[4];
+		double					mF64[4];
 	};
 };
 

Jolt/Math/DVec3.inl

@@ -15,11 +15,25 @@ DVec3::DVec3(Vec3Arg inRHS)
 #if defined(JPH_USE_AVX)
 	mValue = _mm256_cvtps_pd(inRHS.mValue);
 #else
-	mD32[0] = (double)inRHS.GetX();
-	mD32[1] = (double)inRHS.GetY();
-	mD32[2] = (double)inRHS.GetZ();
+	mF64[0] = (double)inRHS.GetX();
+	mF64[1] = (double)inRHS.GetY();
+	mF64[2] = (double)inRHS.GetZ();
 	#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
-		mD32[3] = mD32[2];
+		mF64[3] = mF64[2];
+	#endif
+#endif
+}
+
+DVec3::DVec3(Vec4Arg inRHS)
+{
+#if defined(JPH_USE_AVX)
+	mValue = sFixW(_mm256_cvtps_pd(inRHS.mValue));
+#else
+	mF64[0] = (double)inRHS.GetX();
+	mF64[1] = (double)inRHS.GetY();
+	mF64[2] = (double)inRHS.GetZ();
+	#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+		mF64[3] = mF64[2];
 	#endif
 #endif
 }
@@ -29,29 +43,29 @@ DVec3::DVec3(double inX, double inY, double inZ)
 #if defined(JPH_USE_AVX)
 	mValue = _mm256_set_pd(inZ, inZ, inY, inX); // Assure Z and W are the same
 #else
-	mD32[0] = inX;
-	mD32[1] = inY;
-	mD32[2] = inZ;
+	mF64[0] = inX;
+	mF64[1] = inY;
+	mF64[2] = inZ;
 	#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
-		mD32[3] = mD32[2];
+		mF64[3] = mF64[2];
 	#endif
 #endif
 }
 
-DVec3::DVec3(const double *inV)
+DVec3::DVec3(const Double3 &inV)
 {
 #if defined(JPH_USE_AVX)
-	Type x = _mm256_castpd128_pd256(_mm_load_sd(inV));
-	Type y = _mm256_castpd128_pd256(_mm_load_sd(inV + 1));
-	Type z = _mm256_broadcast_sd(inV + 2);
+	Type x = _mm256_castpd128_pd256(_mm_load_sd(&inV.x));
+	Type y = _mm256_castpd128_pd256(_mm_load_sd(&inV.y));
+	Type z = _mm256_broadcast_sd(&inV.z);
 	Type xy = _mm256_unpacklo_pd(x, y);
 	mValue = _mm256_blend_pd(xy, z, 0b1100); // Assure Z and W are the same
 #else
-	mD32[0] = inV[0];
-	mD32[1] = inV[1];
-	mD32[2] = inV[2];
+	mF64[0] = inV.x;
+	mF64[1] = inV.y;
+	mF64[2] = inV.z;
 	#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
-		mD32[3] = inV[2];
+		mF64[3] = mF64[2];
 	#endif
 #endif
 }
@@ -60,12 +74,12 @@ void DVec3::CheckW() const
 {
 #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
 	// Avoid asserts when both components are NaN
-	JPH_ASSERT(reinterpret_cast<const uint64 *>(mD32)[2] == reinterpret_cast<const uint64 *>(mD32)[3]); 
+	JPH_ASSERT(reinterpret_cast<const uint64 *>(mF64)[2] == reinterpret_cast<const uint64 *>(mF64)[3]); 
 #endif // JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
 } 
 	
 /// Internal helper function that ensures that the Z component is replicated to the W component to prevent divisions by zero
-DVec3::Type DVec3::sFixW(Type inValue)
+DVec3::Type DVec3::sFixW(TypeArg inValue)
 {
 #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
 	#if defined(JPH_USE_AVX)
@@ -85,39 +99,51 @@ DVec3::Type DVec3::sFixW(Type inValue)
 
 DVec3 DVec3::sZero()
 {
-	#if defined(JPH_USE_AVX)
-		return _mm256_setzero_pd();
-	#else
-		return DVec3(0, 0, 0);
-	#endif
+#if defined(JPH_USE_AVX)
+	return _mm256_setzero_pd();
+#else
+	return DVec3(0, 0, 0);
+#endif
 }
 
 DVec3 DVec3::sReplicate(double inV)
 {
-	#if defined(JPH_USE_AVX)
-		return _mm256_set1_pd(inV);
-	#else
-		return DVec3(inV, inV, inV);
-	#endif
+#if defined(JPH_USE_AVX)
+	return _mm256_set1_pd(inV);
+#else
+	return DVec3(inV, inV, inV);
+#endif
 }
 
-DVec3 DVec3::sLoadDouble3Unsafe(const double *inV)
+DVec3 DVec3::sNaN()
 {
-	#if defined(JPH_USE_AVX)
-		Type v = _mm256_loadu_pd(inV);
-	#else
-		Type v = { inV[0], inV[1], inV[2] };
-	#endif
+	return sReplicate(numeric_limits<double>::quiet_NaN());
+}
+
+DVec3 DVec3::sLoadDouble3Unsafe(const Double3 &inV)
+{
+#if defined(JPH_USE_AVX)
+	Type v = _mm256_loadu_pd(&inV.x);
+#else
+	Type v = { inV.x, inV.y, inV.z };
+#endif
 	return sFixW(v);
 }
 
-Vec3 DVec3::ToVec3() const
+void DVec3::StoreDouble3(Double3 *outV) const
 {
-	#if defined(JPH_USE_AVX)
-		return _mm256_cvtpd_ps(mValue);
-	#else
-		return Vec3((float)GetX(), (float)GetY(), (float)GetZ());
-	#endif
+	outV->x = mF64[0];
+	outV->y = mF64[1];
+	outV->z = mF64[2];
+}
+
+DVec3::operator Vec3() const
+{
+#if defined(JPH_USE_AVX)
+	return _mm256_cvtpd_ps(mValue);
+#else
+	return Vec3((float)GetX(), (float)GetY(), (float)GetZ());
+#endif
 }
 
 DVec3 DVec3::sMin(DVec3Arg inV1, DVec3Arg inV2)
@@ -125,9 +151,9 @@ DVec3 DVec3::sMin(DVec3Arg inV1, DVec3Arg inV2)
 #if defined(JPH_USE_AVX)
 	return _mm256_min_pd(inV1.mValue, inV2.mValue);
 #else
-	return DVec3(min(inV1.mD32[0], inV2.mD32[0]), 
-				 min(inV1.mD32[1], inV2.mD32[1]), 
-				 min(inV1.mD32[2], inV2.mD32[2]));
+	return DVec3(min(inV1.mF64[0], inV2.mF64[0]), 
+				 min(inV1.mF64[1], inV2.mF64[1]), 
+				 min(inV1.mF64[2], inV2.mF64[2]));
 #endif
 }
 
@@ -136,9 +162,9 @@ DVec3 DVec3::sMax(DVec3Arg inV1, DVec3Arg inV2)
 #if defined(JPH_USE_AVX)
 	return _mm256_max_pd(inV1.mValue, inV2.mValue);
 #else
-	return DVec3(max(inV1.mD32[0], inV2.mD32[0]), 
-				 max(inV1.mD32[1], inV2.mD32[1]), 
-				 max(inV1.mD32[2], inV2.mD32[2]));
+	return DVec3(max(inV1.mF64[0], inV2.mF64[0]), 
+				 max(inV1.mF64[1], inV2.mF64[1]), 
+				 max(inV1.mF64[2], inV2.mF64[2]));
 #endif
 }
 
@@ -152,9 +178,9 @@ DVec3 DVec3::sEquals(DVec3Arg inV1, DVec3Arg inV2)
 #if defined(JPH_USE_AVX)
 	return _mm256_cmp_pd(inV1.mValue, inV2.mValue, _CMP_EQ_OQ);
 #else
-	return DVec3(inV1.mD32[0] == inV2.mD32[0]? cTrue : cFalse, 
-				 inV1.mD32[1] == inV2.mD32[1]? cTrue : cFalse, 
-				 inV1.mD32[2] == inV2.mD32[2]? cTrue : cFalse);
+	return DVec3(inV1.mF64[0] == inV2.mF64[0]? cTrue : cFalse, 
+				 inV1.mF64[1] == inV2.mF64[1]? cTrue : cFalse, 
+				 inV1.mF64[2] == inV2.mF64[2]? cTrue : cFalse);
 #endif
 }
 
@@ -163,9 +189,9 @@ DVec3 DVec3::sLess(DVec3Arg inV1, DVec3Arg inV2)
 #if defined(JPH_USE_AVX)
 	return _mm256_cmp_pd(inV1.mValue, inV2.mValue, _CMP_LT_OQ);
 #else
-	return DVec3(inV1.mD32[0] < inV2.mD32[0]? cTrue : cFalse, 
-				 inV1.mD32[1] < inV2.mD32[1]? cTrue : cFalse, 
-				 inV1.mD32[2] < inV2.mD32[2]? cTrue : cFalse);
+	return DVec3(inV1.mF64[0] < inV2.mF64[0]? cTrue : cFalse, 
+				 inV1.mF64[1] < inV2.mF64[1]? cTrue : cFalse, 
+				 inV1.mF64[2] < inV2.mF64[2]? cTrue : cFalse);
 #endif
 }
 
@@ -174,9 +200,9 @@ DVec3 DVec3::sLessOrEqual(DVec3Arg inV1, DVec3Arg inV2)
 #if defined(JPH_USE_AVX)
 	return _mm256_cmp_pd(inV1.mValue, inV2.mValue, _CMP_LE_OQ);
 #else
-	return DVec3(inV1.mD32[0] <= inV2.mD32[0]? cTrue : cFalse, 
-				 inV1.mD32[1] <= inV2.mD32[1]? cTrue : cFalse, 
-				 inV1.mD32[2] <= inV2.mD32[2]? cTrue : cFalse);
+	return DVec3(inV1.mF64[0] <= inV2.mF64[0]? cTrue : cFalse, 
+				 inV1.mF64[1] <= inV2.mF64[1]? cTrue : cFalse, 
+				 inV1.mF64[2] <= inV2.mF64[2]? cTrue : cFalse);
 #endif
 }
 
@@ -185,9 +211,9 @@ DVec3 DVec3::sGreater(DVec3Arg inV1, DVec3Arg inV2)
 #if defined(JPH_USE_AVX)
 	return _mm256_cmp_pd(inV1.mValue, inV2.mValue, _CMP_GT_OQ);
 #else
-	return DVec3(inV1.mD32[0] > inV2.mD32[0]? cTrue : cFalse, 
-				 inV1.mD32[1] > inV2.mD32[1]? cTrue : cFalse, 
-				 inV1.mD32[2] > inV2.mD32[2]? cTrue : cFalse);
+	return DVec3(inV1.mF64[0] > inV2.mF64[0]? cTrue : cFalse, 
+				 inV1.mF64[1] > inV2.mF64[1]? cTrue : cFalse, 
+				 inV1.mF64[2] > inV2.mF64[2]? cTrue : cFalse);
 #endif
 }
 
@@ -196,9 +222,9 @@ DVec3 DVec3::sGreaterOrEqual(DVec3Arg inV1, DVec3Arg inV2)
 #if defined(JPH_USE_AVX)
 	return _mm256_cmp_pd(inV1.mValue, inV2.mValue, _CMP_GE_OQ);
 #else
-	return DVec3(inV1.mD32[0] >= inV2.mD32[0]? cTrue : cFalse, 
-				 inV1.mD32[1] >= inV2.mD32[1]? cTrue : cFalse, 
-				 inV1.mD32[2] >= inV2.mD32[2]? cTrue : cFalse);
+	return DVec3(inV1.mF64[0] >= inV2.mF64[0]? cTrue : cFalse, 
+				 inV1.mF64[1] >= inV2.mF64[1]? cTrue : cFalse, 
+				 inV1.mF64[2] >= inV2.mF64[2]? cTrue : cFalse);
 #endif
 }
 
@@ -222,9 +248,9 @@ DVec3 DVec3::sSelect(DVec3Arg inV1, DVec3Arg inV2, DVec3Arg inControl)
 #else
 	DVec3 result;
 	for (int i = 0; i < 3; i++)
-		result.mD32[i] = BitCast<uint64>(inControl.mD32[i])? inV2.mD32[i] : inV1.mD32[i];
+		result.mF64[i] = BitCast<uint64>(inControl.mF64[i])? inV2.mF64[i] : inV1.mF64[i];
 #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
-	result.mD32[3] = result.mD32[2];
+	result.mF64[3] = result.mF64[2];
 #endif // JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
 	return result;
 #endif
@@ -235,9 +261,9 @@ DVec3 DVec3::sOr(DVec3Arg inV1, DVec3Arg inV2)
 #if defined(JPH_USE_AVX)
 	return _mm256_or_pd(inV1.mValue, inV2.mValue);
 #else
-	return DVec3(BitCast<double>(BitCast<uint64>(inV1.mD32[0]) | BitCast<uint64>(inV2.mD32[0])),
-				 BitCast<double>(BitCast<uint64>(inV1.mD32[1]) | BitCast<uint64>(inV2.mD32[1])),
-				 BitCast<double>(BitCast<uint64>(inV1.mD32[2]) | BitCast<uint64>(inV2.mD32[2])));
+	return DVec3(BitCast<double>(BitCast<uint64>(inV1.mF64[0]) | BitCast<uint64>(inV2.mF64[0])),
+				 BitCast<double>(BitCast<uint64>(inV1.mF64[1]) | BitCast<uint64>(inV2.mF64[1])),
+				 BitCast<double>(BitCast<uint64>(inV1.mF64[2]) | BitCast<uint64>(inV2.mF64[2])));
 #endif
 }
 
@@ -246,9 +272,9 @@ DVec3 DVec3::sXor(DVec3Arg inV1, DVec3Arg inV2)
 #if defined(JPH_USE_AVX)
 	return _mm256_xor_pd(inV1.mValue, inV2.mValue);
 #else
-	return DVec3(BitCast<double>(BitCast<uint64>(inV1.mD32[0]) ^ BitCast<uint64>(inV2.mD32[0])),
-				 BitCast<double>(BitCast<uint64>(inV1.mD32[1]) ^ BitCast<uint64>(inV2.mD32[1])),
-				 BitCast<double>(BitCast<uint64>(inV1.mD32[2]) ^ BitCast<uint64>(inV2.mD32[2])));
+	return DVec3(BitCast<double>(BitCast<uint64>(inV1.mF64[0]) ^ BitCast<uint64>(inV2.mF64[0])),
+				 BitCast<double>(BitCast<uint64>(inV1.mF64[1]) ^ BitCast<uint64>(inV2.mF64[1])),
+				 BitCast<double>(BitCast<uint64>(inV1.mF64[2]) ^ BitCast<uint64>(inV2.mF64[2])));
 #endif
 }
 
@@ -257,9 +283,9 @@ DVec3 DVec3::sAnd(DVec3Arg inV1, DVec3Arg inV2)
 #if defined(JPH_USE_AVX)
 	return _mm256_and_pd(inV1.mValue, inV2.mValue);
 #else
-	return DVec3(BitCast<double>(BitCast<uint64>(inV1.mD32[0]) & BitCast<uint64>(inV2.mD32[0])),
-				 BitCast<double>(BitCast<uint64>(inV1.mD32[1]) & BitCast<uint64>(inV2.mD32[1])),
-				 BitCast<double>(BitCast<uint64>(inV1.mD32[2]) & BitCast<uint64>(inV2.mD32[2])));
+	return DVec3(BitCast<double>(BitCast<uint64>(inV1.mF64[0]) & BitCast<uint64>(inV2.mF64[0])),
+				 BitCast<double>(BitCast<uint64>(inV1.mF64[1]) & BitCast<uint64>(inV2.mF64[1])),
+				 BitCast<double>(BitCast<uint64>(inV1.mF64[2]) & BitCast<uint64>(inV2.mF64[2])));
 #endif
 }
 
@@ -268,7 +294,7 @@ int DVec3::GetTrues() const
 #if defined(JPH_USE_AVX)
 	return _mm256_movemask_pd(mValue) & 0x7;
 #else
-	return int((BitCast<uint64>(mD32[0]) >> 63) | ((BitCast<uint64>(mD32[1]) >> 63) << 1) | ((BitCast<uint64>(mD32[2]) >> 63) << 2));
+	return int((BitCast<uint64>(mF64[0]) >> 63) | ((BitCast<uint64>(mF64[1]) >> 63) << 1) | ((BitCast<uint64>(mF64[2]) >> 63) << 2));
 #endif
 }
 
@@ -302,7 +328,7 @@ DVec3 DVec3::operator * (DVec3Arg inV2) const
 #if defined(JPH_USE_AVX)
 	return _mm256_mul_pd(mValue, inV2.mValue);
 #else
-	return DVec3(mD32[0] * inV2.mD32[0], mD32[1] * inV2.mD32[1], mD32[2] * inV2.mD32[2]);
+	return DVec3(mF64[0] * inV2.mF64[0], mF64[1] * inV2.mF64[1], mF64[2] * inV2.mF64[2]);
 #endif
 }
 
@@ -311,7 +337,7 @@ DVec3 DVec3::operator * (double inV2) const
 #if defined(JPH_USE_AVX)
 	return _mm256_mul_pd(mValue, _mm256_set1_pd(inV2));
 #else
-	return DVec3(mD32[0] * inV2, mD32[1] * inV2, mD32[2] * inV2);
+	return DVec3(mF64[0] * inV2, mF64[1] * inV2, mF64[2] * inV2);
 #endif
 }
 
@@ -320,7 +346,7 @@ DVec3 operator * (double inV1, DVec3Arg inV2)
 #if defined(JPH_USE_AVX)
 	return _mm256_mul_pd(_mm256_set1_pd(inV1), inV2.mValue);
 #else
-	return DVec3(inV1 * inV2.mD32[0], inV1 * inV2.mD32[1], inV1 * inV2.mD32[2]);
+	return DVec3(inV1 * inV2.mF64[0], inV1 * inV2.mF64[1], inV1 * inV2.mF64[2]);
 #endif
 }
 
@@ -329,7 +355,7 @@ DVec3 DVec3::operator / (double inV2) const
 #if defined(JPH_USE_AVX)
 	return _mm256_div_pd(mValue, _mm256_set1_pd(inV2));
 #else
-	return DVec3(mD32[0] / inV2, mD32[1] / inV2, mD32[2] / inV2);
+	return DVec3(mF64[0] / inV2, mF64[1] / inV2, mF64[2] / inV2);
 #endif
 }
 
@@ -339,9 +365,9 @@ DVec3 &DVec3::operator *= (double inV2)
 	mValue = _mm256_mul_pd(mValue, _mm256_set1_pd(inV2));
 #else
 	for (int i = 0; i < 3; ++i)
-		mD32[i] *= inV2;
+		mF64[i] *= inV2;
 	#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
-		mD32[3] = mD32[2];
+		mF64[3] = mF64[2];
 	#endif
 #endif
 	return *this;
@@ -353,9 +379,9 @@ DVec3 &DVec3::operator *= (DVec3Arg inV2)
 	mValue = _mm256_mul_pd(mValue, inV2.mValue);
 #else
 	for (int i = 0; i < 3; ++i)
-		mD32[i] *= inV2.mD32[i];
+		mF64[i] *= inV2.mF64[i];
 	#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
-		mD32[3] = mD32[2];
+		mF64[3] = mF64[2];
 	#endif
 #endif
 	return *this;
@@ -367,32 +393,55 @@ DVec3 &DVec3::operator /= (double inV2)
 	mValue = _mm256_div_pd(mValue, _mm256_set1_pd(inV2));
 #else
 	for (int i = 0; i < 3; ++i)
-		mD32[i] /= inV2;
+		mF64[i] /= inV2;
 	#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
-		mD32[3] = mD32[2];
+		mF64[3] = mF64[2];
 	#endif
 #endif
 	return *this;
 }
 
+DVec3 DVec3::operator + (Vec3Arg inV2) const
+{
+#if defined(JPH_USE_AVX)
+	return _mm256_add_pd(mValue, _mm256_cvtps_pd(inV2.mValue));
+#else
+	return DVec3(mF64[0] + inV2.mF32[0], mF64[1] + inV2.mF32[1], mF64[2] + inV2.mF32[2]);
+#endif
+}
+
 DVec3 DVec3::operator + (DVec3Arg inV2) const
 {
 #if defined(JPH_USE_AVX)
 	return _mm256_add_pd(mValue, inV2.mValue);
 #else
-	return DVec3(mD32[0] + inV2.mD32[0], mD32[1] + inV2.mD32[1], mD32[2] + inV2.mD32[2]);
+	return DVec3(mF64[0] + inV2.mF64[0], mF64[1] + inV2.mF64[1], mF64[2] + inV2.mF64[2]);
 #endif
 }
 
+DVec3 &DVec3::operator += (Vec3Arg inV2)
+{
+#if defined(JPH_USE_AVX)
+	mValue = _mm256_add_pd(mValue, _mm256_cvtps_pd(inV2.mValue));
+#else
+	for (int i = 0; i < 3; ++i)
+		mF64[i] += inV2.mF32[i];
+	#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+		mF64[3] = mF64[2];
+	#endif
+#endif
+	return *this;
+}
+
 DVec3 &DVec3::operator += (DVec3Arg inV2)
 {
 #if defined(JPH_USE_AVX)
 	mValue = _mm256_add_pd(mValue, inV2.mValue);
 #else
 	for (int i = 0; i < 3; ++i)
-		mD32[i] += inV2.mD32[i];
+		mF64[i] += inV2.mF64[i];
 	#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
-		mD32[3] = mD32[2];
+		mF64[3] = mF64[2];
 	#endif
 #endif
 	return *this;
@@ -403,7 +452,16 @@ DVec3 DVec3::operator - () const
 #if defined(JPH_USE_AVX)
 	return _mm256_sub_pd(_mm256_setzero_pd(), mValue);
 #else
-	return DVec3(-mD32[0], -mD32[1], -mD32[2]);
+	return DVec3(-mF64[0], -mF64[1], -mF64[2]);
+#endif
+}
+
+DVec3 DVec3::operator - (Vec3Arg inV2) const
+{
+#if defined(JPH_USE_AVX)
+	return _mm256_sub_pd(mValue, _mm256_cvtps_pd(inV2.mValue));
+#else
+	return DVec3(mF64[0] - inV2.mF32[0], mF64[1] - inV2.mF32[1], mF64[2] - inV2.mF32[2]);
 #endif
 }
 
@@ -412,19 +470,33 @@ DVec3 DVec3::operator - (DVec3Arg inV2) const
 #if defined(JPH_USE_AVX)
 	return _mm256_sub_pd(mValue, inV2.mValue);
 #else
-	return DVec3(mD32[0] - inV2.mD32[0], mD32[1] - inV2.mD32[1], mD32[2] - inV2.mD32[2]);
+	return DVec3(mF64[0] - inV2.mF64[0], mF64[1] - inV2.mF64[1], mF64[2] - inV2.mF64[2]);
 #endif
 }
 
+DVec3 &DVec3::operator -= (Vec3Arg inV2)
+{
+#if defined(JPH_USE_AVX)
+	mValue = _mm256_sub_pd(mValue, _mm256_cvtps_pd(inV2.mValue));
+#else
+	for (int i = 0; i < 3; ++i)
+		mF64[i] -= inV2.mF32[i];
+	#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
+		mF64[3] = mF64[2];
+	#endif
+#endif
+	return *this;
+}
+
 DVec3 &DVec3::operator -= (DVec3Arg inV2)
 {
 #if defined(JPH_USE_AVX)
 	mValue = _mm256_sub_pd(mValue, inV2.mValue);
 #else
 	for (int i = 0; i < 3; ++i)
-		mD32[i] -= inV2.mD32[i];
+		mF64[i] -= inV2.mF64[i];
 	#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
-		mD32[3] = mD32[2];
+		mF64[3] = mF64[2];
 	#endif
 #endif
 	return *this;
@@ -436,7 +508,7 @@ DVec3 DVec3::operator / (DVec3Arg inV2) const
 #if defined(JPH_USE_AVX)
 	return _mm256_div_pd(mValue, inV2.mValue);
 #else
-	return DVec3(mD32[0] / inV2.mD32[0], mD32[1] / inV2.mD32[1], mD32[2] / inV2.mD32[2]);
+	return DVec3(mF64[0] / inV2.mF64[0], mF64[1] / inV2.mF64[1], mF64[2] / inV2.mF64[2]);
 #endif
 }
 
@@ -447,7 +519,7 @@ DVec3 DVec3::Abs() const
 #elif defined(JPH_USE_AVX)
 	return _mm256_max_pd(_mm256_sub_pd(_mm256_setzero_pd(), mValue), mValue);
 #else
-	return DVec3(abs(mD32[0]), abs(mD32[1]), abs(mD32[2]));
+	return DVec3(abs(mF64[0]), abs(mF64[1]), abs(mF64[2]));
 #endif
 }
 
@@ -466,9 +538,9 @@ DVec3 DVec3::Cross(DVec3Arg inV2) const
     __m256d t3 = _mm256_sub_pd(t1, t2);
     return _mm256_permute4x64_pd(t3, _MM_SHUFFLE(0, 0, 2, 1)); // Assure Z and W are the same
 #else
-	return DVec3(mD32[1] * inV2.mD32[2] - mD32[2] * inV2.mD32[1],
-				 mD32[2] * inV2.mD32[0] - mD32[0] * inV2.mD32[2],
-				 mD32[0] * inV2.mD32[1] - mD32[1] * inV2.mD32[0]);
+	return DVec3(mF64[1] * inV2.mF64[2] - mF64[2] * inV2.mF64[1],
+				 mF64[2] * inV2.mF64[0] - mF64[0] * inV2.mF64[2],
+				 mF64[0] * inV2.mF64[1] - mF64[1] * inV2.mF64[0]);
 #endif
 }
 
@@ -485,7 +557,7 @@ double DVec3::Dot(DVec3Arg inV2) const
 #else
 	double dot = 0.0;
 	for (int i = 0; i < 3; i++)
-		dot += mD32[i] * inV2.mD32[i];
+		dot += mF64[i] * inV2.mF64[i];
 	return dot;
 #endif
 }
@@ -500,7 +572,7 @@ DVec3 DVec3::Sqrt() const
 #if defined(JPH_USE_AVX)
 	return _mm256_sqrt_pd(mValue);
 #else
-	return DVec3(sqrt(mD32[0]), sqrt(mD32[1]), sqrt(mD32[2]));
+	return DVec3(sqrt(mF64[0]), sqrt(mF64[1]), sqrt(mF64[2]));
 #endif
 }
 
@@ -519,6 +591,15 @@ bool DVec3::IsNormalized(double inTolerance) const
 	return abs(LengthSq() - 1.0) <= inTolerance; 
 }
 
+bool DVec3::IsNaN() const
+{
+#if defined(JPH_USE_AVX)
+	return (_mm256_movemask_pd(_mm256_cmp_pd(mValue, mValue, _CMP_UNORD_Q)) & 0x7) != 0;
+#else
+	return isnan(mF64[0]) || isnan(mF64[1]) || isnan(mF64[2]);
+#endif
+}
+
 DVec3 DVec3::GetSign() const
 {
 #if defined(JPH_USE_AVX)
@@ -526,10 +607,64 @@ DVec3 DVec3::GetSign() const
 	__m256d one = _mm256_set1_pd(1.0);
 	return _mm256_or_pd(_mm256_and_pd(mValue, minus_one), one);
 #else
-	return DVec3(std::signbit(mD32[0])? -1.0 : 1.0, 
-				 std::signbit(mD32[1])? -1.0 : 1.0, 
-				 std::signbit(mD32[2])? -1.0 : 1.0);
+	return DVec3(std::signbit(mF64[0])? -1.0 : 1.0, 
+				 std::signbit(mF64[1])? -1.0 : 1.0, 
+				 std::signbit(mF64[2])? -1.0 : 1.0);
+#endif
+}
+
+DVec3 DVec3::PrepareRoundToZero() const
+{
+	// Float has 23 bit mantissa, double 52 bit mantissa => we lose 29 bits when converting from double to float
+	constexpr uint64 cDoubleToFloatMantissaLoss = (1U << 29) - 1;
+
+#if defined(JPH_USE_AVX)
+	return _mm256_and_pd(mValue, _mm256_castsi256_pd(_mm256_set1_epi64x(int64_t(~cDoubleToFloatMantissaLoss))));
+#else
+	double x = BitCast<double>(BitCast<uint64>(mF64[0]) & ~cDoubleToFloatMantissaLoss);
+	double y = BitCast<double>(BitCast<uint64>(mF64[1]) & ~cDoubleToFloatMantissaLoss);
+	double z = BitCast<double>(BitCast<uint64>(mF64[2]) & ~cDoubleToFloatMantissaLoss);
+
+	return DVec3(x, y, z);
+#endif
+}
+
+DVec3 DVec3::PrepareRoundToInf() const
+{
+	// Float has 23 bit mantissa, double 52 bit mantissa => we lose 29 bits when converting from double to float
+	constexpr uint64 cDoubleToFloatMantissaLoss = (1U << 29) - 1;
+
+#if defined(JPH_USE_AVX)
+	__m256i mantissa_loss = _mm256_set1_epi64x(cDoubleToFloatMantissaLoss);
+	__m256d value_and_mantissa_loss = _mm256_and_pd(mValue, _mm256_castsi256_pd(mantissa_loss));
+	__m256d is_zero = _mm256_cmp_pd(value_and_mantissa_loss, _mm256_setzero_pd(), _CMP_EQ_OQ);
+	__m256d value_or_mantissa_loss = _mm256_or_pd(mValue, _mm256_castsi256_pd(mantissa_loss));
+	return _mm256_blendv_pd(value_or_mantissa_loss, mValue, is_zero);
+#else
+	uint64 ux = BitCast<uint64>(mF64[0]);
+	uint64 uy = BitCast<uint64>(mF64[1]);
+	uint64 uz = BitCast<uint64>(mF64[2]);
+
+	double x = BitCast<double>((ux & cDoubleToFloatMantissaLoss) == 0? ux : (ux | cDoubleToFloatMantissaLoss));
+	double y = BitCast<double>((uy & cDoubleToFloatMantissaLoss) == 0? uy : (uy | cDoubleToFloatMantissaLoss));
+	double z = BitCast<double>((uz & cDoubleToFloatMantissaLoss) == 0? uz : (uz | cDoubleToFloatMantissaLoss));
+
+	return DVec3(x, y, z);
 #endif
 }
 
+Vec3 DVec3::ToVec3RoundDown() const
+{
+	DVec3 to_zero = PrepareRoundToZero();
+	DVec3 to_inf = PrepareRoundToInf();
+	return Vec3(DVec3::sSelect(to_zero, to_inf, DVec3::sLess(*this, DVec3::sZero())));
+}
+
+Vec3 DVec3::ToVec3RoundUp() const
+{
+	DVec3 to_zero = PrepareRoundToZero();
+	DVec3 to_inf = PrepareRoundToInf();
+	return Vec3(DVec3::sSelect(to_inf, to_zero, DVec3::sLess(*this, DVec3::sZero())));
+}
+
 JPH_NAMESPACE_END

Jolt/Math/Double3.h

@@ -0,0 +1,46 @@
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Core/HashCombine.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Class that holds 3 doubles. Used as a storage class. Convert to DVec3 for calculations.
+class [[nodiscard]] Double3
+{
+public:
+	JPH_OVERRIDE_NEW_DELETE
+
+				Double3() = default; ///< Intentionally not initialized for performance reasons
+				Double3(const Double3 &inRHS) = default;
+				Double3(double inX, double inY, double inZ) : x(inX), y(inY), z(inZ) { }
+
+	double		operator [] (int inCoordinate) const	
+	{ 
+		JPH_ASSERT(inCoordinate < 3); 
+		return *(&x + inCoordinate); 
+	}
+
+	bool		operator == (const Double3 &inRHS) const
+	{
+		return x == inRHS.x && y == inRHS.y && z == inRHS.z;
+	}
+
+	bool		operator != (const Double3 &inRHS) const
+	{
+		return x != inRHS.x || y != inRHS.y || z != inRHS.z;
+	}
+
+	double		x;
+	double		y;
+	double		z;
+};
+
+static_assert(is_trivial<Double3>(), "Is supposed to be a trivial type!");
+
+JPH_NAMESPACE_END
+
+// Create a std::hash for Double3
+JPH_MAKE_HASHABLE(JPH::Double3, t.x, t.y, t.z)

Jolt/Math/Mat44.h

@@ -16,9 +16,13 @@ public:
 	// Underlying column type
 	using Type = Vec4::Type;
 
+	// Argument type
+	using ArgType = Mat44Arg;
+
 	/// Constructor
 								Mat44() = default; ///< Intentionally not initialized for performance reasons
 	JPH_INLINE					Mat44(Vec4Arg inC1, Vec4Arg inC2, Vec4Arg inC3, Vec4Arg inC4);
+	JPH_INLINE					Mat44(Vec4Arg inC1, Vec4Arg inC2, Vec4Arg inC3, Vec3Arg inC4);
 								Mat44(const Mat44 &inM2) = default;
 	JPH_INLINE					Mat44(Type inC1, Type inC2, Type inC3, Type inC4);
 
@@ -208,6 +212,11 @@ public:
 	/// will be made orthogonal using the modified Gram-Schmidt algorithm (see: https://en.wikipedia.org/wiki/Gram%E2%80%93Schmidt_process)
 	JPH_INLINE Mat44			Decompose(Vec3 &outScale) const;
 
+#ifndef JPH_DOUBLE_PRECISION
+	/// In single precision mode just return the matrix itself
+	JPH_INLINE Mat44			ToMat44() const											{ return *this; }
+#endif // !JPH_DOUBLE_PRECISION
+
 	/// To String
 	friend ostream &			operator << (ostream &inStream, Mat44Arg inM)
 	{

Jolt/Math/Mat44.inl

@@ -16,6 +16,11 @@ Mat44::Mat44(Vec4Arg inC1, Vec4Arg inC2, Vec4Arg inC3, Vec4Arg inC4) :
 { 
 }
 
+Mat44::Mat44(Vec4Arg inC1, Vec4Arg inC2, Vec4Arg inC3, Vec3Arg inC4) : 
+	mCol { inC1, inC2, inC3, Vec4(inC4, 1.0f) } 
+{ 
+}
+
 Mat44::Mat44(Type inC1, Type inC2, Type inC3, Type inC4) : 
 	mCol { inC1, inC2, inC3, inC4 } 
 {

Jolt/Math/MathTypes.h

@@ -13,15 +13,21 @@ class Vec8;
 class UVec8;
 class Quat;
 class Mat44;
+class DMat44;
 
 // Types to use for passing arguments to functions
 using Vec3Arg = Vec3;
-using DVec3Arg = DVec3;
+#ifdef JPH_USE_AVX
+	using DVec3Arg = DVec3;
+#else
+	using DVec3Arg = const DVec3 &;
+#endif
 using Vec4Arg = Vec4;
 using UVec4Arg = UVec4;
 using Vec8Arg = Vec8;
 using UVec8Arg = UVec8;
 using QuatArg = Quat;
 using Mat44Arg = const Mat44 &;
+using DMat44Arg = const DMat44 &;
 
 JPH_NAMESPACE_END

Jolt/Math/Real.h

@@ -0,0 +1,43 @@
+// SPDX-FileCopyrightText: 2022 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Math/DVec3.h>
+#include <Jolt/Math/DMat44.h>
+
+JPH_NAMESPACE_BEGIN
+
+#ifdef JPH_DOUBLE_PRECISION
+
+// Define real to double
+using Real = double;
+using Real3 = Double3;
+using RVec3 = DVec3;
+using RVec3Arg = DVec3Arg;
+using RMat44 = DMat44;
+using RMat44Arg = DMat44Arg;
+
+#define JPH_RVECTOR_ALIGNMENT JPH_DVECTOR_ALIGNMENT
+
+#else
+
+// Define real to float
+using Real = float;
+using Real3 = Float3;
+using RVec3  = Vec3;
+using RVec3Arg = Vec3Arg;
+using RMat44 = Mat44;
+using RMat44Arg = Mat44Arg;
+
+#define JPH_RVECTOR_ALIGNMENT JPH_VECTOR_ALIGNMENT
+
+#endif // JPH_DOUBLE_PRECISION
+
+// Put the 'real' operator in a namespace so that users can opt in to use it:
+// using namespace JPH::literals;
+namespace literals {
+	constexpr Real operator "" _r (long double inValue) { return Real(inValue); }
+};
+
+JPH_NAMESPACE_END

Jolt/Math/Vec3.h

@@ -25,6 +25,9 @@ public:
 	using Type = Vec4::Type;
 #endif
 
+	// Argument type
+	using ArgType = Vec3Arg;
+
 	/// Constructor
 								Vec3() = default; ///< Intentionally not initialized for performance reasons
 								Vec3(const Vec3 &inRHS) = default;

Jolt/Math/Vec3.inl

@@ -766,8 +766,9 @@ void Vec3::StoreFloat3(Float3 *outV) const
     vst1_f32(&outV->x, xy);
     vst1q_lane_f32(&outV->z, mValue, 2);
 #else
-	for (int i = 0; i < 3; ++i)
-		(&outV->x)[i] = mF32[i];
+	outV->x = mF32[0];
+	outV->y = mF32[1];
+	outV->z = mF32[2];
 #endif
 }
 

Jolt/ObjectStream/ObjectStream.h

@@ -50,13 +50,17 @@ public:
 	virtual bool				ReadPrimitiveData(uint32 &outPrimitive) = 0;
 	virtual bool				ReadPrimitiveData(uint64 &outPrimitive) = 0;
 	virtual bool				ReadPrimitiveData(float &outPrimitive) = 0;
+	virtual bool				ReadPrimitiveData(double &outPrimitive) = 0;
 	virtual bool				ReadPrimitiveData(bool &outPrimitive) = 0;
 	virtual bool				ReadPrimitiveData(String &outPrimitive) = 0;
 	virtual bool				ReadPrimitiveData(Float3 &outPrimitive) = 0;
+	virtual bool				ReadPrimitiveData(Double3 &outPrimitive) = 0;
 	virtual bool				ReadPrimitiveData(Vec3 &outPrimitive) = 0;
+	virtual bool				ReadPrimitiveData(DVec3 &outPrimitive) = 0;
 	virtual bool				ReadPrimitiveData(Vec4 &outPrimitive) = 0;
 	virtual bool				ReadPrimitiveData(Quat &outPrimitive) = 0;
 	virtual bool				ReadPrimitiveData(Mat44 &outPrimitive) = 0;
+	virtual bool				ReadPrimitiveData(DMat44 &outPrimitive) = 0;
 
 	///@name Read compounds
 	virtual bool				ReadClassData(const char *inClassName, void *inInstance) = 0;
@@ -80,13 +84,17 @@ public:
 	virtual void				WritePrimitiveData(const uint32 &inPrimitive) = 0;
 	virtual void				WritePrimitiveData(const uint64 &inPrimitive) = 0;
 	virtual void				WritePrimitiveData(const float &inPrimitive) = 0;
+	virtual void				WritePrimitiveData(const double &inPrimitive) = 0;
 	virtual void				WritePrimitiveData(const bool &inPrimitive) = 0;
 	virtual void				WritePrimitiveData(const String &inPrimitive) = 0;
 	virtual void				WritePrimitiveData(const Float3 &inPrimitive) = 0;
+	virtual void				WritePrimitiveData(const Double3 &inPrimitive) = 0;
 	virtual void				WritePrimitiveData(const Vec3 &inPrimitive) = 0;
+	virtual void				WritePrimitiveData(const DVec3 &inPrimitive) = 0;
 	virtual void				WritePrimitiveData(const Vec4 &inPrimitive) = 0;
 	virtual void				WritePrimitiveData(const Quat &inPrimitive) = 0;
 	virtual void				WritePrimitiveData(const Mat44 &inPrimitive) = 0;
+	virtual void				WritePrimitiveData(const DMat44 &inPrimitive) = 0;
 
 	///@name Write compounds
 	virtual void				WritePointerData(const RTTI *inRTTI, const void *inPointer) = 0;

Jolt/ObjectStream/ObjectStreamBinaryIn.cpp

@@ -98,6 +98,15 @@ bool ObjectStreamBinaryIn::ReadPrimitiveData(float &outPrimitive)
 	return true;
 }
 
+bool ObjectStreamBinaryIn::ReadPrimitiveData(double &outPrimitive)
+{
+	double primitive;
+	mStream.read((char *)&primitive, sizeof(primitive));
+	if (mStream.fail()) return false;
+	outPrimitive = primitive;
+	return true;
+}
+
 bool ObjectStreamBinaryIn::ReadPrimitiveData(bool &outPrimitive)
 {
 	bool primitive;
@@ -153,6 +162,15 @@ bool ObjectStreamBinaryIn::ReadPrimitiveData(Float3 &outPrimitive)
 	return true;
 }
 
+bool ObjectStreamBinaryIn::ReadPrimitiveData(Double3 &outPrimitive)
+{
+	Double3 primitive;
+	mStream.read((char *)&primitive, sizeof(Double3));
+	if (mStream.fail()) return false;
+	outPrimitive = primitive;
+	return true;
+}
+
 bool ObjectStreamBinaryIn::ReadPrimitiveData(Vec3 &outPrimitive)
 {
 	Float3 primitive;
@@ -162,6 +180,15 @@ bool ObjectStreamBinaryIn::ReadPrimitiveData(Vec3 &outPrimitive)
 	return true;
 }
 
+bool ObjectStreamBinaryIn::ReadPrimitiveData(DVec3 &outPrimitive)
+{
+	Double3 primitive;
+	mStream.read((char *)&primitive, sizeof(Double3));
+	if (mStream.fail()) return false;
+	outPrimitive = DVec3(primitive); // Use Float3 constructor so that we initialize W too
+	return true;
+}
+
 bool ObjectStreamBinaryIn::ReadPrimitiveData(Vec4 &outPrimitive)
 {
 	Vec4 primitive;
@@ -189,4 +216,14 @@ bool ObjectStreamBinaryIn::ReadPrimitiveData(Mat44 &outPrimitive)
 	return true;
 }
 
+bool ObjectStreamBinaryIn::ReadPrimitiveData(DMat44 &outPrimitive)
+{
+	Vec4 c0, c1, c2;
+	DVec3 c3;
+	if (!ReadPrimitiveData(c0) || !ReadPrimitiveData(c1) || !ReadPrimitiveData(c2) || !ReadPrimitiveData(c3))
+		return false;
+	outPrimitive = DMat44(c0, c1, c2, c3);
+	return true;
+}
+
 JPH_NAMESPACE_END

Jolt/ObjectStream/ObjectStreamBinaryIn.h

@@ -28,13 +28,17 @@ public:
 	virtual bool				ReadPrimitiveData(uint32 &outPrimitive) override;
 	virtual bool				ReadPrimitiveData(uint64 &outPrimitive) override;
 	virtual bool				ReadPrimitiveData(float &outPrimitive) override;
+	virtual bool				ReadPrimitiveData(double &outPrimitive) override;
 	virtual bool				ReadPrimitiveData(bool &outPrimitive) override;
 	virtual bool				ReadPrimitiveData(String &outPrimitive) override;
 	virtual bool				ReadPrimitiveData(Float3 &outPrimitive) override;
+	virtual bool				ReadPrimitiveData(Double3 &outPrimitive) override;
 	virtual bool				ReadPrimitiveData(Vec3 &outPrimitive) override;
+	virtual bool				ReadPrimitiveData(DVec3 &outPrimitive) override;
 	virtual bool				ReadPrimitiveData(Vec4 &outPrimitive) override;
 	virtual bool				ReadPrimitiveData(Quat &outPrimitive) override;
 	virtual bool				ReadPrimitiveData(Mat44 &outPrimitive) override;
+	virtual bool				ReadPrimitiveData(DMat44 &outPrimitive) override;
 
 private:
 	using StringTable = UnorderedMap<uint32, String>;

Jolt/ObjectStream/ObjectStreamBinaryOut.cpp

@@ -66,6 +66,11 @@ void ObjectStreamBinaryOut::WritePrimitiveData(const float &inPrimitive)
 	mStream.write((const char *)&inPrimitive, sizeof(inPrimitive));
 }
 
+void ObjectStreamBinaryOut::WritePrimitiveData(const double &inPrimitive)
+{
+	mStream.write((const char *)&inPrimitive, sizeof(inPrimitive));
+}
+
 void ObjectStreamBinaryOut::WritePrimitiveData(const bool &inPrimitive)
 {
 	mStream.write((const char *)&inPrimitive, sizeof(inPrimitive));
@@ -103,11 +108,21 @@ void ObjectStreamBinaryOut::WritePrimitiveData(const Float3 &inPrimitive)
 	mStream.write((const char *)&inPrimitive, sizeof(Float3));
 }
 
+void ObjectStreamBinaryOut::WritePrimitiveData(const Double3 &inPrimitive)
+{
+	mStream.write((const char *)&inPrimitive, sizeof(Double3));
+}
+
 void ObjectStreamBinaryOut::WritePrimitiveData(const Vec3 &inPrimitive)
 {
 	mStream.write((const char *)&inPrimitive, 3 * sizeof(float));
 }
 
+void ObjectStreamBinaryOut::WritePrimitiveData(const DVec3 &inPrimitive)
+{
+	mStream.write((const char *)&inPrimitive, 3 * sizeof(double));
+}
+
 void ObjectStreamBinaryOut::WritePrimitiveData(const Vec4 &inPrimitive)
 {
 	mStream.write((const char *)&inPrimitive, sizeof(inPrimitive));
@@ -123,4 +138,12 @@ void ObjectStreamBinaryOut::WritePrimitiveData(const Mat44 &inPrimitive)
 	mStream.write((const char *)&inPrimitive, sizeof(inPrimitive));
 }
 
+void ObjectStreamBinaryOut::WritePrimitiveData(const DMat44 &inPrimitive)
+{
+	WritePrimitiveData(inPrimitive.GetColumn4(0));
+	WritePrimitiveData(inPrimitive.GetColumn4(1));
+	WritePrimitiveData(inPrimitive.GetColumn4(2));
+	WritePrimitiveData(inPrimitive.GetTranslation());
+}
+
 JPH_NAMESPACE_END

Jolt/ObjectStream/ObjectStreamBinaryOut.h

@@ -28,13 +28,17 @@ public:
 	virtual void				WritePrimitiveData(const uint32 &inPrimitive) override;
 	virtual void				WritePrimitiveData(const uint64 &inPrimitive) override;
 	virtual void				WritePrimitiveData(const float &inPrimitive) override;
+	virtual void				WritePrimitiveData(const double &inPrimitive) override;
 	virtual void				WritePrimitiveData(const bool &inPrimitive) override;
 	virtual void				WritePrimitiveData(const String &inPrimitive) override;
 	virtual void				WritePrimitiveData(const Float3 &inPrimitive) override;
+	virtual void				WritePrimitiveData(const Double3 &inPrimitive) override;
 	virtual void				WritePrimitiveData(const Vec3 &inPrimitive) override;
+	virtual void				WritePrimitiveData(const DVec3 &inPrimitive) override;
 	virtual void				WritePrimitiveData(const Vec4 &inPrimitive) override;
 	virtual void				WritePrimitiveData(const Quat &inPrimitive) override;
 	virtual void				WritePrimitiveData(const Mat44 &inPrimitive) override;
+	virtual void				WritePrimitiveData(const DMat44 &inPrimitive) override;
 
 private:
 	using StringTable = UnorderedMap<String, uint32>;

Jolt/ObjectStream/ObjectStreamIn.cpp

@@ -278,19 +278,19 @@ bool ObjectStreamIn::ReadRTTI()
 			return false;
 		
 		// Read type
-		if (!ReadDataType(attribute.mDataType)) 
+		if (!ReadDataType(attribute.mSourceType)) 
 			return false;
 
 		// Read array depth
-		while (attribute.mDataType == EOSDataType::Array) 
+		while (attribute.mSourceType == EOSDataType::Array) 
 		{
 			++attribute.mArrayDepth;
-			if (!ReadDataType(attribute.mDataType)) 
+			if (!ReadDataType(attribute.mSourceType)) 
 				return false;
 		}
 
 		// Read instance/pointer class name
-		if ((attribute.mDataType == EOSDataType::Instance || attribute.mDataType == EOSDataType::Pointer) 
+		if ((attribute.mSourceType == EOSDataType::Instance || attribute.mSourceType == EOSDataType::Pointer) 
 			&& !ReadName(attribute.mClassName)) 
 			return false;
 
@@ -312,8 +312,26 @@ bool ObjectStreamIn::ReadRTTI()
 			if (attribute.mIndex >= 0)
 			{
 				const SerializableAttribute &attr = rtti->GetAttribute(attribute.mIndex);
-				if (!attr.IsType(attribute.mArrayDepth, attribute.mDataType, attribute.mClassName.c_str()))
+				if (attr.IsType(attribute.mArrayDepth, attribute.mSourceType, attribute.mClassName.c_str()))
+				{
+					// No conversion needed
+					attribute.mDestinationType = attribute.mSourceType;
+				}
+				else if (attribute.mArrayDepth == 0 && attribute.mClassName.empty())
+				{
+					// Try to apply type conversions
+					if (attribute.mSourceType == EOSDataType::T_Vec3 && attr.IsType(0, EOSDataType::T_DVec3, ""))
+						attribute.mDestinationType = EOSDataType::T_DVec3;
+					else if (attribute.mSourceType == EOSDataType::T_DVec3 && attr.IsType(0, EOSDataType::T_Vec3, ""))
+						attribute.mDestinationType = EOSDataType::T_Vec3;
+					else
+						attribute.mIndex = -1;
+				}
+				else
+				{
+					// No conversion exists
 					attribute.mIndex = -1;
+				}
 			}
 		} 
 
@@ -345,10 +363,34 @@ bool ObjectStreamIn::ReadClassData(const ClassDescription &inClassDesc, void *in
 		if (attr_desc.mIndex >= 0 && inInstance)
 		{
 			const SerializableAttribute &attr = inClassDesc.mRTTI->GetAttribute(attr_desc.mIndex);
-			continue_reading = attr.ReadData(*this, inInstance);
+			if (attr_desc.mSourceType ==  attr_desc.mDestinationType)
+			{
+				continue_reading = attr.ReadData(*this, inInstance);
+			}
+			else if (attr_desc.mSourceType == EOSDataType::T_Vec3 && attr_desc.mDestinationType == EOSDataType::T_DVec3)
+			{
+				// Vec3 to DVec3
+				Vec3 tmp;
+				continue_reading = ReadPrimitiveData(tmp);
+				if (continue_reading)
+					*attr.GetMemberPointer<DVec3>(inInstance) = DVec3(tmp);
+			}
+			else if (attr_desc.mSourceType == EOSDataType::T_DVec3 && attr_desc.mDestinationType == EOSDataType::T_Vec3)
+			{
+				// DVec3 to Vec3
+				DVec3 tmp;
+				continue_reading = ReadPrimitiveData(tmp);
+				if (continue_reading)
+					*attr.GetMemberPointer<Vec3>(inInstance) = Vec3(tmp);
+			}
+			else
+			{
+				JPH_ASSERT(false); // Unknown conversion
+				continue_reading = SkipAttributeData(attr_desc.mArrayDepth, attr_desc.mSourceType, attr_desc.mClassName.c_str());
+			}
 		}
 		else
-			continue_reading = SkipAttributeData(attr_desc.mArrayDepth, attr_desc.mDataType, attr_desc.mClassName.c_str());
+			continue_reading = SkipAttributeData(attr_desc.mArrayDepth, attr_desc.mSourceType, attr_desc.mClassName.c_str());
 
 		if (!continue_reading)
 			break;
@@ -478,6 +520,13 @@ bool ObjectStreamIn::SkipAttributeData(int inArrayDepth, EOSDataType inDataType,
 						break;
 					}
 				
+				case EOSDataType::T_double:
+					{	
+						double temporary;
+						continue_reading = ReadPrimitiveData(temporary);
+						break;
+					}
+				
 				case EOSDataType::T_bool:
 					{	
 						bool temporary;
@@ -499,6 +548,13 @@ bool ObjectStreamIn::SkipAttributeData(int inArrayDepth, EOSDataType inDataType,
 						break;
 					}
 
+				case EOSDataType::T_Double3:
+					{	
+						Double3 temporary;
+						continue_reading = ReadPrimitiveData(temporary);
+						break;
+					}
+
 				case EOSDataType::T_Vec3:
 					{	
 						Vec3 temporary;
@@ -506,6 +562,13 @@ bool ObjectStreamIn::SkipAttributeData(int inArrayDepth, EOSDataType inDataType,
 						break;
 					}
 
+				case EOSDataType::T_DVec3:
+					{	
+						DVec3 temporary;
+						continue_reading = ReadPrimitiveData(temporary);
+						break;
+					}
+
 				case EOSDataType::T_Vec4:
 					{	
 						Vec4 temporary;
@@ -527,6 +590,13 @@ bool ObjectStreamIn::SkipAttributeData(int inArrayDepth, EOSDataType inDataType,
 						break;
 					}
 
+				case EOSDataType::T_DMat44:
+					{	
+						DMat44 temporary;
+						continue_reading = ReadPrimitiveData(temporary);
+						break;
+					}
+
 				case EOSDataType::Array:
 				case EOSDataType::Object:
 				case EOSDataType::Declare:

Jolt/ObjectStream/ObjectStreamIn.h

@@ -100,7 +100,8 @@ private:
 	struct AttributeDescription
 	{
 		int						mArrayDepth = 0;
-		EOSDataType				mDataType = EOSDataType::Invalid;
+		EOSDataType				mSourceType = EOSDataType::Invalid;
+		EOSDataType				mDestinationType = EOSDataType::Invalid;
 		String					mClassName;
 		int						mIndex = -1;
 	};

Jolt/ObjectStream/ObjectStreamTextIn.cpp

@@ -40,20 +40,28 @@ bool ObjectStreamTextIn::ReadDataType(EOSDataType &outType)
 			outType  = EOSDataType::T_uint64;
 		else if (token == "float")
 			outType  = EOSDataType::T_float;
+		else if (token == "double")
+			outType  = EOSDataType::T_double;
 		else if (token == "bool")
 			outType  = EOSDataType::T_bool;
 		else if (token == "string")
 			outType  = EOSDataType::T_String;
 		else if (token == "float3")
 			outType  = EOSDataType::T_Float3;
+		else if (token == "double3")
+			outType  = EOSDataType::T_Double3;
 		else if (token == "vec3")
 			outType  = EOSDataType::T_Vec3;
+		else if (token == "dvec3")
+			outType  = EOSDataType::T_DVec3;
 		else if (token == "vec4")
 			outType  = EOSDataType::T_Vec4;
 		else if (token == "quat")
 			outType  = EOSDataType::T_Quat;
 		else if (token == "mat44")
 			outType  = EOSDataType::T_Mat44;
+		else if (token == "dmat44")
+			outType  = EOSDataType::T_DMat44;
 		else
 		{
 			Trace("ObjectStreamTextIn: Found unknown data type.");
@@ -160,6 +168,16 @@ bool ObjectStreamTextIn::ReadPrimitiveData(float &outPrimitive)
 	return !stream.fail();
 }
 
+bool ObjectStreamTextIn::ReadPrimitiveData(double &outPrimitive)
+{
+	String token;
+	if (!ReadWord(token))
+		return false;
+	IStringStream stream(token);
+	stream >> outPrimitive;
+	return !stream.fail();
+}
+
 bool ObjectStreamTextIn::ReadPrimitiveData(bool &outPrimitive)
 {
 	String token;
@@ -269,6 +287,15 @@ bool ObjectStreamTextIn::ReadPrimitiveData(Float3 &outPrimitive)
 	return true;
 }
 
+bool ObjectStreamTextIn::ReadPrimitiveData(Double3 &outPrimitive)
+{
+	double x, y, z;
+	if (!ReadPrimitiveData(x) || !ReadPrimitiveData(y) || !ReadPrimitiveData(z))
+		return false;
+	outPrimitive = Double3(x, y, z);
+	return true;
+}
+
 bool ObjectStreamTextIn::ReadPrimitiveData(Vec3 &outPrimitive)
 {
 	float x, y, z;
@@ -278,6 +305,15 @@ bool ObjectStreamTextIn::ReadPrimitiveData(Vec3 &outPrimitive)
 	return true;
 }
 
+bool ObjectStreamTextIn::ReadPrimitiveData(DVec3 &outPrimitive)
+{
+	double x, y, z;
+	if (!ReadPrimitiveData(x) || !ReadPrimitiveData(y) || !ReadPrimitiveData(z))
+		return false;
+	outPrimitive = DVec3(x, y, z);
+	return true;
+}
+
 bool ObjectStreamTextIn::ReadPrimitiveData(Vec4 &outPrimitive)
 {
 	float x, y, z, w;
@@ -305,6 +341,16 @@ bool ObjectStreamTextIn::ReadPrimitiveData(Mat44 &outPrimitive)
 	return true;
 }
 
+bool ObjectStreamTextIn::ReadPrimitiveData(DMat44 &outPrimitive)
+{
+	Vec4 c0, c1, c2;
+	DVec3 c3;
+	if (!ReadPrimitiveData(c0) || !ReadPrimitiveData(c1) || !ReadPrimitiveData(c2) || !ReadPrimitiveData(c3))
+		return false;
+	outPrimitive = DMat44(c0, c1, c2, c3);
+	return true;
+}
+
 bool ObjectStreamTextIn::ReadChar(char &outChar)
 {
 	mStream.get(outChar);

Jolt/ObjectStream/ObjectStreamTextIn.h

@@ -28,13 +28,17 @@ public:
 	virtual bool				ReadPrimitiveData(uint32 &outPrimitive) override;
 	virtual bool				ReadPrimitiveData(uint64 &outPrimitive) override;
 	virtual bool				ReadPrimitiveData(float &outPrimitive) override;
+	virtual bool				ReadPrimitiveData(double &outPrimitive) override;
 	virtual bool				ReadPrimitiveData(bool &outPrimitive) override;
 	virtual bool				ReadPrimitiveData(String &outPrimitive) override;
 	virtual bool				ReadPrimitiveData(Float3 &outPrimitive) override;
+	virtual bool				ReadPrimitiveData(Double3 &outPrimitive) override;
 	virtual bool				ReadPrimitiveData(Vec3 &outPrimitive) override;
+	virtual bool				ReadPrimitiveData(DVec3 &outPrimitive) override;
 	virtual bool				ReadPrimitiveData(Vec4 &outPrimitive) override;
 	virtual bool				ReadPrimitiveData(Quat &outPrimitive) override;
 	virtual bool				ReadPrimitiveData(Mat44 &outPrimitive) override;
+	virtual bool				ReadPrimitiveData(DMat44 &outPrimitive) override;
 
 private:
 	bool						ReadChar(char &outChar);

Jolt/ObjectStream/ObjectStreamTextOut.cpp

@@ -29,13 +29,17 @@ void ObjectStreamTextOut::WriteDataType(EOSDataType inType)
 	case EOSDataType::T_uint32:		WriteWord("uint32");		break;
 	case EOSDataType::T_uint64:		WriteWord("uint64");		break;
 	case EOSDataType::T_float:		WriteWord("float");			break;
+	case EOSDataType::T_double:		WriteWord("double");		break;
 	case EOSDataType::T_bool:		WriteWord("bool");			break;
 	case EOSDataType::T_String:		WriteWord("string");		break;
 	case EOSDataType::T_Float3:		WriteWord("float3");		break;
+	case EOSDataType::T_Double3:	WriteWord("double3");		break;
 	case EOSDataType::T_Vec3:		WriteWord("vec3");			break;
+	case EOSDataType::T_DVec3:		WriteWord("dvec3");			break;
 	case EOSDataType::T_Vec4:		WriteWord("vec4");			break;
 	case EOSDataType::T_Quat:		WriteWord("quat");			break;
 	case EOSDataType::T_Mat44:		WriteWord("mat44");			break;
+	case EOSDataType::T_DMat44:		WriteWord("dmat44");		break;
 	case EOSDataType::Invalid:
 	default:						JPH_ASSERT(false);			break;
 	}
@@ -89,6 +93,14 @@ void ObjectStreamTextOut::WritePrimitiveData(const float &inPrimitive)
 	WriteWord(stream.str());
 }
 
+void ObjectStreamTextOut::WritePrimitiveData(const double &inPrimitive)
+{
+	std::ostringstream stream;
+	stream.precision(17);
+	stream << inPrimitive;
+	WriteWord(stream.str());
+}
+
 void ObjectStreamTextOut::WritePrimitiveData(const bool &inPrimitive)
 {
 	WriteWord(inPrimitive? "true" : "false");
@@ -103,6 +115,15 @@ void ObjectStreamTextOut::WritePrimitiveData(const Float3 &inPrimitive)
 	WritePrimitiveData(inPrimitive.z);
 }
 
+void ObjectStreamTextOut::WritePrimitiveData(const Double3 &inPrimitive)
+{
+	WritePrimitiveData(inPrimitive.x);
+	WriteChar(' ');
+	WritePrimitiveData(inPrimitive.y);
+	WriteChar(' ');
+	WritePrimitiveData(inPrimitive.z);
+}
+
 void ObjectStreamTextOut::WritePrimitiveData(const Vec3 &inPrimitive)
 {
 	WritePrimitiveData(inPrimitive.GetX());
@@ -112,6 +133,15 @@ void ObjectStreamTextOut::WritePrimitiveData(const Vec3 &inPrimitive)
 	WritePrimitiveData(inPrimitive.GetZ());
 }
 
+void ObjectStreamTextOut::WritePrimitiveData(const DVec3 &inPrimitive)
+{
+	WritePrimitiveData(inPrimitive.GetX());
+	WriteChar(' ');
+	WritePrimitiveData(inPrimitive.GetY());
+	WriteChar(' ');
+	WritePrimitiveData(inPrimitive.GetZ());
+}
+
 void ObjectStreamTextOut::WritePrimitiveData(const Vec4 &inPrimitive)
 {
 	WritePrimitiveData(inPrimitive.GetX());
@@ -145,6 +175,17 @@ void ObjectStreamTextOut::WritePrimitiveData(const Mat44 &inPrimitive)
 	WritePrimitiveData(inPrimitive.GetColumn4(3));
 }
 
+void ObjectStreamTextOut::WritePrimitiveData(const DMat44 &inPrimitive)
+{
+	WritePrimitiveData(inPrimitive.GetColumn4(0));
+	WriteChar(' ');
+	WritePrimitiveData(inPrimitive.GetColumn4(1));
+	WriteChar(' ');
+	WritePrimitiveData(inPrimitive.GetColumn4(2));
+	WriteChar(' ');
+	WritePrimitiveData(inPrimitive.GetTranslation());
+}
+
 void ObjectStreamTextOut::WritePrimitiveData(const String &inPrimitive)
 {
 	String temporary(inPrimitive);

Jolt/ObjectStream/ObjectStreamTextOut.h

@@ -28,13 +28,17 @@ public:
 	virtual void				WritePrimitiveData(const uint32 &inPrimitive) override;
 	virtual void				WritePrimitiveData(const uint64 &inPrimitive) override;
 	virtual void				WritePrimitiveData(const float &inPrimitive) override;
+	virtual void				WritePrimitiveData(const double &inPrimitive) override;
 	virtual void				WritePrimitiveData(const bool &inPrimitive) override;
 	virtual void				WritePrimitiveData(const String &inPrimitive) override;
 	virtual void				WritePrimitiveData(const Float3 &inPrimitive) override;
+	virtual void				WritePrimitiveData(const Double3 &inPrimitive) override;
 	virtual void				WritePrimitiveData(const Vec3 &inPrimitive) override;
+	virtual void				WritePrimitiveData(const DVec3 &inPrimitive) override;
 	virtual void				WritePrimitiveData(const Vec4 &inPrimitive) override;
 	virtual void				WritePrimitiveData(const Quat &inPrimitive) override;
 	virtual void				WritePrimitiveData(const Mat44 &inPrimitive) override;
+	virtual void				WritePrimitiveData(const DMat44 &inPrimitive) override;
 
 	///@name Layout hints (for text output)
 	virtual void				HintNextItem() override;

Jolt/ObjectStream/ObjectStreamTypes.h

@@ -1,6 +1,7 @@
 // SPDX-FileCopyrightText: 2021 Jorrit Rouwe
 // SPDX-License-Identifier: MIT
 
+// Note: Order is important, an enum is created and its value is stored in a binary stream!
 JPH_DECLARE_PRIMITIVE(uint8)
 JPH_DECLARE_PRIMITIVE(uint16)
 JPH_DECLARE_PRIMITIVE(int)
@@ -14,5 +15,9 @@ JPH_DECLARE_PRIMITIVE(Vec3)
 JPH_DECLARE_PRIMITIVE(Vec4)
 JPH_DECLARE_PRIMITIVE(Quat)
 JPH_DECLARE_PRIMITIVE(Mat44)
+JPH_DECLARE_PRIMITIVE(double)
+JPH_DECLARE_PRIMITIVE(DVec3)
+JPH_DECLARE_PRIMITIVE(DMat44)
+JPH_DECLARE_PRIMITIVE(Double3)
 
 #undef JPH_DECLARE_PRIMITIVE

Jolt/ObjectStream/SerializableAttribute.h

@@ -50,6 +50,12 @@ public:
 	void						SetName(const char *inName)							{ mName = inName; }
 	const char *				GetName() const										{ return mName; }
 
+	/// Access to the memory location that contains the member
+	template <class T>
+	inline T *					GetMemberPointer(void *inObject) const				{ return reinterpret_cast<T *>(reinterpret_cast<uint8 *>(inObject) + mMemberOffset); }
+	template <class T>
+	inline const T *			GetMemberPointer(const void *inObject) const		{ return reinterpret_cast<const T *>(reinterpret_cast<const uint8 *>(inObject) + mMemberOffset); }
+
 	/// In case this attribute contains an RTTI type, return it (note that a Array<sometype> will return the rtti of sometype)
 	const RTTI *				GetMemberPrimitiveType() const
 	{
@@ -65,13 +71,13 @@ public:
 	/// Read the data for this attribute into attribute containing class inObject
 	bool						ReadData(IObjectStreamIn &ioStream, void *inObject) const
 	{
-		return mReadData(ioStream, reinterpret_cast<uint8 *>(inObject) + mMemberOffset);
+		return mReadData(ioStream, GetMemberPointer<void>(inObject));
 	}
 
 	/// Write the data for this attribute from attribute containing class inObject
 	void						WriteData(IObjectStreamOut &ioStream, const void *inObject) const
 	{
-		mWriteData(ioStream, reinterpret_cast<const uint8 *>(inObject) + mMemberOffset);
+		mWriteData(ioStream, GetMemberPointer<void>(inObject));
 	}
 
 	/// Write the data type of this attribute to a stream

Jolt/ObjectStream/TypeDeclarations.cpp

@@ -13,13 +13,17 @@ JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(int)			{ }
 JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(uint32)		{ }
 JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(uint64)		{ }
 JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(float)			{ }
+JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(double)		{ }
 JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(bool)			{ }
 JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(String)		{ }
 JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(Float3)		{ }
+JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(Double3)		{ }
 JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(Vec3)			{ }
+JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(DVec3)			{ }
 JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(Vec4)			{ }
 JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(Quat)			{ }
 JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(Mat44)			{ }
+JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(DMat44)		{ }
 
 JPH_IMPLEMENT_SERIALIZABLE_OUTSIDE_CLASS(Color)			
 { 

Jolt/ObjectStream/TypeDeclarations.h

@@ -17,13 +17,17 @@ JPH_DECLARE_RTTI_OUTSIDE_CLASS(int);
 JPH_DECLARE_RTTI_OUTSIDE_CLASS(uint32);
 JPH_DECLARE_RTTI_OUTSIDE_CLASS(uint64);
 JPH_DECLARE_RTTI_OUTSIDE_CLASS(float);
+JPH_DECLARE_RTTI_OUTSIDE_CLASS(double);
 JPH_DECLARE_RTTI_OUTSIDE_CLASS(bool);
 JPH_DECLARE_RTTI_OUTSIDE_CLASS(String);
 JPH_DECLARE_RTTI_OUTSIDE_CLASS(Float3);
+JPH_DECLARE_RTTI_OUTSIDE_CLASS(Double3);
 JPH_DECLARE_RTTI_OUTSIDE_CLASS(Vec3);
+JPH_DECLARE_RTTI_OUTSIDE_CLASS(DVec3);
 JPH_DECLARE_RTTI_OUTSIDE_CLASS(Vec4);
 JPH_DECLARE_RTTI_OUTSIDE_CLASS(Quat);
 JPH_DECLARE_RTTI_OUTSIDE_CLASS(Mat44);
+JPH_DECLARE_RTTI_OUTSIDE_CLASS(DMat44);
 JPH_DECLARE_SERIALIZABLE_OUTSIDE_CLASS(Color);
 JPH_DECLARE_SERIALIZABLE_OUTSIDE_CLASS(AABox);
 JPH_DECLARE_SERIALIZABLE_OUTSIDE_CLASS(Triangle);

Jolt/Physics/Body/Body.cpp

@@ -74,16 +74,16 @@ void Body::SetAllowSleeping(bool inAllow)
 		ResetSleepTestSpheres();
 }
 
-void Body::MoveKinematic(Vec3Arg inTargetPosition, QuatArg inTargetRotation, float inDeltaTime)
+void Body::MoveKinematic(RVec3Arg inTargetPosition, QuatArg inTargetRotation, float inDeltaTime)
 {
 	JPH_ASSERT(!IsStatic());
 	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read)); 
 
 	// Calculate center of mass at end situation
-	Vec3 new_com = inTargetPosition + inTargetRotation * mShape->GetCenterOfMass();
+	RVec3 new_com = inTargetPosition + inTargetRotation * mShape->GetCenterOfMass();
 
 	// Calculate delta position and rotation
-	Vec3 delta_pos = new_com - mPosition;
+	Vec3 delta_pos = Vec3(new_com - mPosition);
 	Quat delta_rotation = inTargetRotation * mRotation.Conjugated();
 
 	mMotionProperties->MoveKinematic(delta_pos, delta_rotation, inDeltaTime);
@@ -94,7 +94,7 @@ void Body::CalculateWorldSpaceBoundsInternal()
 	mBounds = mShape->GetWorldSpaceBounds(GetCenterOfMassTransform(), Vec3::sReplicate(1.0f));
 }
 
-void Body::SetPositionAndRotationInternal(Vec3Arg inPosition, QuatArg inRotation) 
+void Body::SetPositionAndRotationInternal(RVec3Arg inPosition, QuatArg inRotation) 
 { 
 	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::ReadWrite)); 
 
@@ -143,15 +143,27 @@ Body::ECanSleep Body::UpdateSleepStateInternal(float inDeltaTime, float inMaxMov
 		return ECanSleep::CannotSleep;
 
 	// Get the points to test
-	Vec3 points[3];
+	RVec3 points[3];
 	GetSleepTestPoints(points);
 
+#ifdef JPH_DOUBLE_PRECISION
+	// Get base offset for spheres
+	DVec3 offset = mMotionProperties->GetSleepTestOffset();
+#endif // JPH_DOUBLE_PRECISION
+
 	for (int i = 0; i < 3; ++i)
 	{
 		Sphere &sphere = mMotionProperties->mSleepTestSpheres[i];
 
+		// Make point relative to base offset
+#ifdef JPH_DOUBLE_PRECISION
+		Vec3 p = Vec3(points[i] - offset);
+#else
+		Vec3 p = points[i];
+#endif // JPH_DOUBLE_PRECISION
+
 		// Encapsulate the point in a sphere
-		sphere.EncapsulatePoint(points[i]);
+		sphere.EncapsulatePoint(p);
 
 		// Test if it exceeded the max movement
 		if (sphere.GetRadius() > inMaxMovement)
@@ -166,17 +178,21 @@ Body::ECanSleep Body::UpdateSleepStateInternal(float inDeltaTime, float inMaxMov
 	return mMotionProperties->mSleepTestTimer >= inTimeBeforeSleep? ECanSleep::CanSleep : ECanSleep::CannotSleep;
 }
 
-bool Body::ApplyBuoyancyImpulse(const Plane &inSurface, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime)
+bool Body::ApplyBuoyancyImpulse(RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNormal, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime)
 {
 	JPH_PROFILE_FUNCTION();
 
 	// We follow the approach from 'Game Programming Gems 6' 2.5 Exact Buoyancy for Polyhedra
 	// All quantities below are in world space
 
+	// For GetSubmergedVolume we transform the surface relative to the body position for increased precision
+	Mat44 rotation = Mat44::sRotation(mRotation);
+	Plane surface_relative_to_body = Plane::sFromPointAndNormal(inSurfacePosition - mPosition, inSurfaceNormal);
+
 	// Calculate amount of volume that is submerged and what the center of buoyancy is
 	float total_volume, submerged_volume;
-	Vec3 center_of_buoyancy;
-	mShape->GetSubmergedVolume(GetCenterOfMassTransform(), Vec3::sReplicate(1.0f), inSurface, total_volume, submerged_volume, center_of_buoyancy);
+	Vec3 relative_center_of_buoyancy;
+	mShape->GetSubmergedVolume(rotation, Vec3::sReplicate(1.0f), surface_relative_to_body, total_volume, submerged_volume, relative_center_of_buoyancy JPH_IF_DEBUG_RENDERER(, mPosition));
 		
 	// If we're not submerged, there's no point in doing the rest of the calculations
 	if (submerged_volume > 0.0f)
@@ -185,6 +201,7 @@ bool Body::ApplyBuoyancyImpulse(const Plane &inSurface, float inBuoyancy, float
 		// Draw submerged volume properties
 		if (Shape::sDrawSubmergedVolumes)
 		{
+			RVec3 center_of_buoyancy = mPosition + relative_center_of_buoyancy;
 			DebugRenderer::sInstance->DrawMarker(center_of_buoyancy, Color::sWhite, 2.0f);
 			DebugRenderer::sInstance->DrawText3D(center_of_buoyancy, StringFormat("%.3f / %.3f", (double)submerged_volume, (double)total_volume));
 		}
@@ -201,7 +218,6 @@ bool Body::ApplyBuoyancyImpulse(const Plane &inSurface, float inBuoyancy, float
 		Vec3 buoyancy_impulse = -fluid_density * submerged_volume * mMotionProperties->GetGravityFactor() * inGravity * inDeltaTime;
 
 		// Calculate the velocity of the center of buoyancy relative to the fluid
-		Vec3 relative_center_of_buoyancy = center_of_buoyancy - mPosition;
 		Vec3 linear_velocity = mMotionProperties->GetLinearVelocity();
 		Vec3 angular_velocity = mMotionProperties->GetAngularVelocity();
 		Vec3 center_of_buoyancy_velocity = linear_velocity + angular_velocity.Cross(relative_center_of_buoyancy);

Jolt/Physics/Body/Body.h

@@ -116,13 +116,13 @@ public:
 	inline Vec3				GetPointVelocityCOM(Vec3Arg inPointRelativeToCOM) const			{ return !IsStatic()? mMotionProperties->GetPointVelocityCOM(inPointRelativeToCOM) : Vec3::sZero(); }
 
 	/// Velocity of point inPoint (in world space, e.g. on the surface of the body) of the body (unit: m/s)
-	inline Vec3				GetPointVelocity(Vec3Arg inPoint) const							{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read)); return GetPointVelocityCOM(inPoint - mPosition); }
+	inline Vec3				GetPointVelocity(RVec3Arg inPoint) const						{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read)); return GetPointVelocityCOM(Vec3(inPoint - mPosition)); }
 
 	/// Add force (unit: N) at center of mass for the next time step, will be reset after the next call to PhysicsSimulation::Update
 	inline void				AddForce(Vec3Arg inForce)										{ JPH_ASSERT(IsDynamic()); (Vec3::sLoadFloat3Unsafe(mMotionProperties->mForce) + inForce).StoreFloat3(&mMotionProperties->mForce); }
 
 	/// Add force (unit: N) at inPosition for the next time step, will be reset after the next call to PhysicsSimulation::Update
-	inline void				AddForce(Vec3Arg inForce, Vec3Arg inPosition);
+	inline void				AddForce(Vec3Arg inForce, RVec3Arg inPosition);
 
 	/// Add torque (unit: N m) for the next time step, will be reset after the next call to PhysicsSimulation::Update
 	inline void				AddTorque(Vec3Arg inTorque)										{ JPH_ASSERT(IsDynamic()); (Vec3::sLoadFloat3Unsafe(mMotionProperties->mTorque) + inTorque).StoreFloat3(&mMotionProperties->mTorque); }
@@ -140,16 +140,17 @@ public:
 	inline void				AddImpulse(Vec3Arg inImpulse);
 
 	/// Add impulse to point in world space (unit: kg m/s)
-	inline void				AddImpulse(Vec3Arg inImpulse, Vec3Arg inPosition);
+	inline void				AddImpulse(Vec3Arg inImpulse, RVec3Arg inPosition);
 
 	/// Add angular impulse in world space (unit: N m s)
 	inline void				AddAngularImpulse(Vec3Arg inAngularImpulse);
 	
 	/// Set velocity of body such that it will be positioned at inTargetPosition/Rotation in inDeltaTime seconds.
-	void					MoveKinematic(Vec3Arg inTargetPosition, QuatArg inTargetRotation, float inDeltaTime);
+	void					MoveKinematic(RVec3Arg inTargetPosition, QuatArg inTargetRotation, float inDeltaTime);
 
 	/// Applies an impulse to the body that simulates fluid buoyancy and drag
-	/// @param inSurface The fluid surface (normal should point up) in world space
+	/// @param inSurfacePosition Position on the fluid surface in world space
+	/// @param inSurfaceNormal Normal of the fluid surface (should point up)
 	/// @param inBuoyancy The buoyancy factor for the body. 1 = neutral body, < 1 sinks, > 1 floats. Note that we don't use the fluid density since it is harder to configure than a simple number between [0, 2]
 	/// @param inLinearDrag Linear drag factor that slows down the body when in the fluid (approx. 0.5)
 	/// @param inAngularDrag Angular drag factor that slows down rotation when the body is in the fluid (approx. 0.01)
@@ -157,7 +158,7 @@ public:
 	/// @param inGravity The graviy vector (pointing down)
 	/// @param inDeltaTime Delta time of the next simulation step (in s)
 	/// @return true if an impulse was applied, false if the body was not in the fluid
-	bool					ApplyBuoyancyImpulse(const Plane &inSurface, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime);
+	bool					ApplyBuoyancyImpulse(RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNormal, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime);
 
 	/// Check if this body has been added to the physics system
 	inline bool				IsInBroadPhase() const											{ return (mFlags.load(memory_order_relaxed) & uint8(EFlags::IsInBroadPhase)) != 0; }
@@ -169,22 +170,22 @@ public:
 	inline const Shape *	GetShape() const												{ return mShape; }
 
 	/// World space position of the body
-	inline Vec3				GetPosition() const												{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read)); return mPosition - mRotation * mShape->GetCenterOfMass(); }
+	inline RVec3			GetPosition() const												{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read)); return mPosition - mRotation * mShape->GetCenterOfMass(); }
 
 	/// World space rotation of the body
 	inline Quat 			GetRotation() const												{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read)); return mRotation; }
 
 	/// Calculates the transform of this body
-	inline Mat44			GetWorldTransform() const;
+	inline RMat44			GetWorldTransform() const;
 
 	/// Gets the world space position of this body's center of mass
-	inline Vec3 			GetCenterOfMassPosition() const									{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read)); return mPosition; }
+	inline RVec3 			GetCenterOfMassPosition() const									{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read)); return mPosition; }
 
 	/// Calculates the transform for this body's center of mass
-	inline Mat44			GetCenterOfMassTransform() const;
+	inline RMat44			GetCenterOfMassTransform() const;
 
 	/// Calculates the inverse of the transform for this body's center of mass
-	inline Mat44			GetInverseCenterOfMassTransform() const;
+	inline RMat44			GetInverseCenterOfMassTransform() const;
 
 	/// Get world space bounding box
 	inline const AABox &	GetWorldSpaceBounds() const										{ return mBounds; }
@@ -202,7 +203,7 @@ public:
 	void					SetUserData(uint64 inUserData)									{ mUserData = inUserData; }
 
 	/// Get surface normal of a particular sub shape and its world space surface position on this body
-	inline Vec3				GetWorldSpaceSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inPosition) const;
+	inline Vec3				GetWorldSpaceSurfaceNormal(const SubShapeID &inSubShapeID, RVec3Arg inPosition) const;
 
 	/// Get the transformed shape of this body, which can be used to do collision detection outside of a body lock
 	inline TransformedShape	GetTransformedShape() const										{ JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read)); return TransformedShape(mPosition, mRotation, mShape, mID); }
@@ -241,7 +242,7 @@ public:
 	void					CalculateWorldSpaceBoundsInternal();
 
 	/// Function to update body's position (should only be called by the BodyInterface since it also requires updating the broadphase)
-	void					SetPositionAndRotationInternal(Vec3Arg inPosition, QuatArg inRotation);
+	void					SetPositionAndRotationInternal(RVec3Arg inPosition, QuatArg inRotation);
 
 	/// Updates the center of mass and optionally mass propertes after shifting the center of mass or changes to the shape (should only be called by the BodyInterface since it also requires updating the broadphase)
 	/// @param inPreviousCenterOfMass Center of mass of the shape before the alterations
@@ -280,7 +281,7 @@ private:
 
 	explicit				Body(bool);														///< Alternative constructor that initializes all members
 
-	inline void				GetSleepTestPoints(Vec3 *outPoints) const;						///< Determine points to test for checking if body is sleeping: COM, COM + largest bounding box axis, COM + second largest bounding box axis
+	inline void				GetSleepTestPoints(RVec3 *outPoints) const;						///< Determine points to test for checking if body is sleeping: COM, COM + largest bounding box axis, COM + second largest bounding box axis
 	inline void				ResetSleepTestSpheres();										///< Reset spheres to current position as returned by GetSleepTestPoints
 
 	enum class EFlags : uint8
@@ -291,7 +292,7 @@ private:
 	};
 
 	// 16 byte aligned
-	Vec3					mPosition;														///< World space position of center of mass
+	RVec3					mPosition;														///< World space position of center of mass
 	Quat					mRotation;														///< World space rotation of center of mass
 	AABox					mBounds;														///< World space bounding box of the body
 
@@ -314,16 +315,20 @@ private:
 	EMotionType				mMotionType;													///< Type of motion (static, dynamic or kinematic)
 	atomic<uint8>			mFlags = 0;														///< See EFlags for possible flags
 	
-	// 121 bytes up to here (64-bit mode)
+	// 121 bytes up to here (64-bit mode, single precision)
 
 #if JPH_CPU_ADDRESS_BITS == 32
 	// Padding for 32 bit mode
 	char					mPadding[19];
 #endif
+#ifdef JPH_DOUBLE_PRECISION
+	// Padding to align to 256 bit
+	char					mPadding2[16];
+#endif
 };
 
-static_assert(sizeof(Body) == 128, "Body should be 128 bytes");
-static_assert(alignof(Body) == JPH_VECTOR_ALIGNMENT, "Body should properly align");
+static_assert(sizeof(Body) == JPH_IF_SINGLE_PRECISION_ELSE(128, 160), "Body size is incorrect");
+static_assert(alignof(Body) == JPH_RVECTOR_ALIGNMENT, "Body should properly align");
 
 JPH_NAMESPACE_END
 

Jolt/Physics/Body/Body.inl

@@ -5,25 +5,25 @@
 
 JPH_NAMESPACE_BEGIN
 
-Mat44 Body::GetWorldTransform() const
+RMat44 Body::GetWorldTransform() const
 {
 	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read)); 
 
-	return Mat44::sRotationTranslation(mRotation, mPosition).PreTranslated(-mShape->GetCenterOfMass());
+	return RMat44::sRotationTranslation(mRotation, mPosition).PreTranslated(-mShape->GetCenterOfMass());
 }
 
-Mat44 Body::GetCenterOfMassTransform() const
+RMat44 Body::GetCenterOfMassTransform() const
 {
 	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read)); 
 
-	return Mat44::sRotationTranslation(mRotation, mPosition);
+	return RMat44::sRotationTranslation(mRotation, mPosition);
 }
 
-Mat44 Body::GetInverseCenterOfMassTransform() const
+RMat44 Body::GetInverseCenterOfMassTransform() const
 {
 	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read)); 
 
-	return Mat44::sInverseRotationTranslation(mRotation, mPosition);
+	return RMat44::sInverseRotationTranslation(mRotation, mPosition);
 }
 
 inline bool Body::sFindCollidingPairsCanCollide(const Body &inBody1, const Body &inBody2)
@@ -108,10 +108,10 @@ void Body::SubRotationStep(Vec3Arg inAngularVelocityTimesDeltaTime)
 	}
 }
 
-Vec3 Body::GetWorldSpaceSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inPosition) const
+Vec3 Body::GetWorldSpaceSurfaceNormal(const SubShapeID &inSubShapeID, RVec3Arg inPosition) const
 {
-	Mat44 inv_com = GetInverseCenterOfMassTransform();
-	return inv_com.Multiply3x3Transposed(mShape->GetSurfaceNormal(inSubShapeID, inv_com * inPosition)).Normalized();
+	RMat44 inv_com = GetInverseCenterOfMassTransform();
+	return inv_com.Multiply3x3Transposed(mShape->GetSurfaceNormal(inSubShapeID, Vec3(inv_com * inPosition))).Normalized();
 }
 
 Mat44 Body::GetInverseInertia() const
@@ -121,10 +121,10 @@ Mat44 Body::GetInverseInertia() const
 	return GetMotionProperties()->GetInverseInertiaForRotation(Mat44::sRotation(mRotation));
 }
 
-void Body::AddForce(Vec3Arg inForce, Vec3Arg inPosition)
+void Body::AddForce(Vec3Arg inForce, RVec3Arg inPosition)
 {
 	AddForce(inForce);
-	AddTorque((inPosition - mPosition).Cross(inForce));
+	AddTorque(Vec3(inPosition - mPosition).Cross(inForce));
 }
 
 void Body::AddImpulse(Vec3Arg inImpulse)
@@ -134,13 +134,13 @@ void Body::AddImpulse(Vec3Arg inImpulse)
 	SetLinearVelocityClamped(mMotionProperties->GetLinearVelocity() + inImpulse * mMotionProperties->GetInverseMass());
 }
 
-void Body::AddImpulse(Vec3Arg inImpulse, Vec3Arg inPosition)
+void Body::AddImpulse(Vec3Arg inImpulse, RVec3Arg inPosition)
 {
 	JPH_ASSERT(IsDynamic());
 
 	SetLinearVelocityClamped(mMotionProperties->GetLinearVelocity() + inImpulse * mMotionProperties->GetInverseMass());
 
-	SetAngularVelocityClamped(mMotionProperties->GetAngularVelocity() + GetInverseInertia() * (inPosition - mPosition).Cross(inImpulse));
+	SetAngularVelocityClamped(mMotionProperties->GetAngularVelocity() + GetInverseInertia() * Vec3(inPosition - mPosition).Cross(inImpulse));
 }
 
 void Body::AddAngularImpulse(Vec3Arg inAngularImpulse)
@@ -150,7 +150,7 @@ void Body::AddAngularImpulse(Vec3Arg inAngularImpulse)
 	SetAngularVelocityClamped(mMotionProperties->GetAngularVelocity() + GetInverseInertia() * inAngularImpulse);
 }
 
-void Body::GetSleepTestPoints(Vec3 *outPoints) const
+void Body::GetSleepTestPoints(RVec3 *outPoints) const
 {
 	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read)); 
 
@@ -186,7 +186,7 @@ void Body::GetSleepTestPoints(Vec3 *outPoints) const
 
 void Body::ResetSleepTestSpheres()
 {
-	Vec3 points[3];
+	RVec3 points[3];
 	GetSleepTestPoints(points);
 	mMotionProperties->ResetSleepTestSpheres(points);
 }

Jolt/Physics/Body/BodyCreationSettings.h

@@ -31,8 +31,8 @@ public:
 
 	/// Constructor
 							BodyCreationSettings() = default;
-							BodyCreationSettings(const ShapeSettings *inShape, Vec3Arg inPosition, QuatArg inRotation, EMotionType inMotionType, ObjectLayer inObjectLayer) : mPosition(inPosition), mRotation(inRotation), mObjectLayer(inObjectLayer), mMotionType(inMotionType), mShape(inShape) { }
-							BodyCreationSettings(const Shape *inShape, Vec3Arg inPosition, QuatArg inRotation, EMotionType inMotionType, ObjectLayer inObjectLayer) : mPosition(inPosition), mRotation(inRotation), mObjectLayer(inObjectLayer), mMotionType(inMotionType), mShapePtr(inShape) { }
+							BodyCreationSettings(const ShapeSettings *inShape, RVec3Arg inPosition, QuatArg inRotation, EMotionType inMotionType, ObjectLayer inObjectLayer) : mPosition(inPosition), mRotation(inRotation), mObjectLayer(inObjectLayer), mMotionType(inMotionType), mShape(inShape) { }
+							BodyCreationSettings(const Shape *inShape, RVec3Arg inPosition, QuatArg inRotation, EMotionType inMotionType, ObjectLayer inObjectLayer) : mPosition(inPosition), mRotation(inRotation), mObjectLayer(inObjectLayer), mMotionType(inMotionType), mShapePtr(inShape) { }
 
 	/// Access to the shape settings object. This contains serializable (non-runtime optimized) information about the Shape.
 	const ShapeSettings *	GetShapeSettings() const										{ return mShape; }
@@ -74,7 +74,7 @@ public:
 	/// Restore a shape, all its children and materials. Pass in an empty map in ioShapeMap / ioMaterialMap / ioGroupFilterMap or reuse the same map while reading multiple shapes from the same stream in order to restore duplicates.
 	static BCSResult		sRestoreWithChildren(StreamIn &inStream, IDToShapeMap &ioShapeMap, IDToMaterialMap &ioMaterialMap, IDToGroupFilterMap &ioGroupFilterMap);
 
-	Vec3					mPosition = Vec3::sZero();										///< Position of the body (not of the center of mass)
+	RVec3					mPosition = RVec3::sZero();										///< Position of the body (not of the center of mass)
 	Quat					mRotation = Quat::sIdentity();									///< Rotation of the body
 	Vec3					mLinearVelocity = Vec3::sZero();								///< World space linear velocity of the center of mass (m/s)
 	Vec3					mAngularVelocity = Vec3::sZero();								///< World space angular velocity (rad/s)

Jolt/Physics/Body/BodyInterface.cpp

@@ -326,7 +326,7 @@ ObjectLayer BodyInterface::GetObjectLayer(const BodyID &inBodyID) const
 		return cObjectLayerInvalid;
 }
 
-void BodyInterface::SetPositionAndRotation(const BodyID &inBodyID, Vec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode)
+void BodyInterface::SetPositionAndRotation(const BodyID &inBodyID, RVec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode)
 {
 	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
 	if (lock.Succeeded())
@@ -349,7 +349,7 @@ void BodyInterface::SetPositionAndRotation(const BodyID &inBodyID, Vec3Arg inPos
 	}
 }
 
-void BodyInterface::SetPositionAndRotationWhenChanged(const BodyID &inBodyID, Vec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode)
+void BodyInterface::SetPositionAndRotationWhenChanged(const BodyID &inBodyID, RVec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode)
 {
 	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
 	if (lock.Succeeded())
@@ -377,7 +377,7 @@ void BodyInterface::SetPositionAndRotationWhenChanged(const BodyID &inBodyID, Ve
 	}
 }
 
-void BodyInterface::GetPositionAndRotation(const BodyID &inBodyID, Vec3 &outPosition, Quat &outRotation) const
+void BodyInterface::GetPositionAndRotation(const BodyID &inBodyID, RVec3 &outPosition, Quat &outRotation) const
 {
 	BodyLockRead lock(*mBodyLockInterface, inBodyID);
 	if (lock.Succeeded())
@@ -388,12 +388,12 @@ void BodyInterface::GetPositionAndRotation(const BodyID &inBodyID, Vec3 &outPosi
 	}
 	else
 	{
-		outPosition = Vec3::sZero();
+		outPosition = RVec3::sZero();
 		outRotation = Quat::sIdentity();
 	}
 }
 
-void BodyInterface::SetPosition(const BodyID &inBodyID, Vec3Arg inPosition, EActivation inActivationMode)
+void BodyInterface::SetPosition(const BodyID &inBodyID, RVec3Arg inPosition, EActivation inActivationMode)
 {
 	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
 	if (lock.Succeeded())
@@ -416,22 +416,22 @@ void BodyInterface::SetPosition(const BodyID &inBodyID, Vec3Arg inPosition, EAct
 	}
 }
 
-Vec3 BodyInterface::GetPosition(const BodyID &inBodyID) const
+RVec3 BodyInterface::GetPosition(const BodyID &inBodyID) const
 {
 	BodyLockRead lock(*mBodyLockInterface, inBodyID);
 	if (lock.Succeeded())
 		return lock.GetBody().GetPosition();
 	else
-		return Vec3::sZero();
+		return RVec3::sZero();
 }
 
-Vec3 BodyInterface::GetCenterOfMassPosition(const BodyID &inBodyID) const
+RVec3 BodyInterface::GetCenterOfMassPosition(const BodyID &inBodyID) const
 {
 	BodyLockRead lock(*mBodyLockInterface, inBodyID);
 	if (lock.Succeeded())
 		return lock.GetBody().GetCenterOfMassPosition();
 	else
-		return Vec3::sZero();
+		return RVec3::sZero();
 }
 
 void BodyInterface::SetRotation(const BodyID &inBodyID, QuatArg inRotation, EActivation inActivationMode)
@@ -466,25 +466,25 @@ Quat BodyInterface::GetRotation(const BodyID &inBodyID) const
 		return Quat::sIdentity();
 }
 
-Mat44 BodyInterface::GetWorldTransform(const BodyID &inBodyID) const
+RMat44 BodyInterface::GetWorldTransform(const BodyID &inBodyID) const
 {
 	BodyLockRead lock(*mBodyLockInterface, inBodyID);
 	if (lock.Succeeded())
 		return lock.GetBody().GetWorldTransform();
 	else
-		return Mat44::sIdentity();
+		return RMat44::sIdentity();
 }
 
-Mat44 BodyInterface::GetCenterOfMassTransform(const BodyID &inBodyID) const
+RMat44 BodyInterface::GetCenterOfMassTransform(const BodyID &inBodyID) const
 {
 	BodyLockRead lock(*mBodyLockInterface, inBodyID);
 	if (lock.Succeeded())
 		return lock.GetBody().GetCenterOfMassTransform();
 	else
-		return Mat44::sIdentity();
+		return RMat44::sIdentity();
 }
 
-void BodyInterface::MoveKinematic(const BodyID &inBodyID, Vec3Arg inTargetPosition, QuatArg inTargetRotation, float inDeltaTime)
+void BodyInterface::MoveKinematic(const BodyID &inBodyID, RVec3Arg inTargetPosition, QuatArg inTargetRotation, float inDeltaTime)
 {
 	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
 	if (lock.Succeeded())
@@ -623,7 +623,7 @@ Vec3 BodyInterface::GetAngularVelocity(const BodyID &inBodyID) const
 	return Vec3::sZero();
 }
 
-Vec3 BodyInterface::GetPointVelocity(const BodyID &inBodyID, Vec3Arg inPoint) const
+Vec3 BodyInterface::GetPointVelocity(const BodyID &inBodyID, RVec3Arg inPoint) const
 {
 	BodyLockRead lock(*mBodyLockInterface, inBodyID);
 	if (lock.Succeeded())
@@ -652,7 +652,7 @@ void BodyInterface::AddForce(const BodyID &inBodyID, Vec3Arg inForce)
 	}
 }
 
-void BodyInterface::AddForce(const BodyID &inBodyID, Vec3Arg inForce, Vec3Arg inPoint)
+void BodyInterface::AddForce(const BodyID &inBodyID, Vec3Arg inForce, RVec3Arg inPoint)
 {
 	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
 	if (lock.Succeeded())
@@ -717,7 +717,7 @@ void BodyInterface::AddImpulse(const BodyID &inBodyID, Vec3Arg inImpulse)
 	}
 }
 
-void BodyInterface::AddImpulse(const BodyID &inBodyID, Vec3Arg inImpulse, Vec3Arg inPoint)
+void BodyInterface::AddImpulse(const BodyID &inBodyID, Vec3Arg inImpulse, RVec3Arg inPoint)
 {
 	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
 	if (lock.Succeeded())
@@ -749,7 +749,7 @@ void BodyInterface::AddAngularImpulse(const BodyID &inBodyID, Vec3Arg inAngularI
 	}
 }
 
-void BodyInterface::SetPositionRotationAndVelocity(const BodyID &inBodyID, Vec3Arg inPosition, QuatArg inRotation, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity)
+void BodyInterface::SetPositionRotationAndVelocity(const BodyID &inBodyID, RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity)
 {
 	BodyLockWrite lock(*mBodyLockInterface, inBodyID);
 	if (lock.Succeeded())

Jolt/Physics/Body/BodyInterface.h

@@ -146,20 +146,20 @@ public:
 
 	///@name Position and rotation of a body
 	///@{
-	void						SetPositionAndRotation(const BodyID &inBodyID, Vec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode);
-	void						SetPositionAndRotationWhenChanged(const BodyID &inBodyID, Vec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode); ///< Will only update the position/rotation and activate the body when the difference is larger than a very small number. This avoids updating the broadphase/waking up a body when the resulting position/orientation doesn't really change.
-	void						GetPositionAndRotation(const BodyID &inBodyID, Vec3 &outPosition, Quat &outRotation) const;
-	void						SetPosition(const BodyID &inBodyID, Vec3Arg inPosition, EActivation inActivationMode);
-	Vec3						GetPosition(const BodyID &inBodyID) const;
-	Vec3						GetCenterOfMassPosition(const BodyID &inBodyID) const;
+	void						SetPositionAndRotation(const BodyID &inBodyID, RVec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode);
+	void						SetPositionAndRotationWhenChanged(const BodyID &inBodyID, RVec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode); ///< Will only update the position/rotation and activate the body when the difference is larger than a very small number. This avoids updating the broadphase/waking up a body when the resulting position/orientation doesn't really change.
+	void						GetPositionAndRotation(const BodyID &inBodyID, RVec3 &outPosition, Quat &outRotation) const;
+	void						SetPosition(const BodyID &inBodyID, RVec3Arg inPosition, EActivation inActivationMode);
+	RVec3						GetPosition(const BodyID &inBodyID) const;
+	RVec3						GetCenterOfMassPosition(const BodyID &inBodyID) const;
 	void						SetRotation(const BodyID &inBodyID, QuatArg inRotation, EActivation inActivationMode);
 	Quat						GetRotation(const BodyID &inBodyID) const;
-	Mat44						GetWorldTransform(const BodyID &inBodyID) const;
-	Mat44						GetCenterOfMassTransform(const BodyID &inBodyID) const;
+	RMat44						GetWorldTransform(const BodyID &inBodyID) const;
+	RMat44						GetCenterOfMassTransform(const BodyID &inBodyID) const;
 	///@}
 
 	/// Set velocity of body such that it will be positioned at inTargetPosition/Rotation in inDeltaTime seconds (will activate body if needed)
-	void						MoveKinematic(const BodyID &inBodyID, Vec3Arg inTargetPosition, QuatArg inTargetRotation, float inDeltaTime);
+	void						MoveKinematic(const BodyID &inBodyID, RVec3Arg inTargetPosition, QuatArg inTargetRotation, float inDeltaTime);
 
 	/// Linear or angular velocity (functions will activate body if needed).
 	/// Note that the linear velocity is the velocity of the center of mass, which may not coincide with the position of your object, to correct for this: \f$VelocityCOM = Velocity - AngularVelocity \times ShapeCOM\f$
@@ -171,16 +171,16 @@ public:
 	void						AddLinearAndAngularVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity); ///< Add linear and angular to current velocities
 	void						SetAngularVelocity(const BodyID &inBodyID, Vec3Arg inAngularVelocity);
 	Vec3						GetAngularVelocity(const BodyID &inBodyID) const;
-	Vec3						GetPointVelocity(const BodyID &inBodyID, Vec3Arg inPoint) const; ///< Velocity of point inPoint (in world space, e.g. on the surface of the body) of the body
+	Vec3						GetPointVelocity(const BodyID &inBodyID, RVec3Arg inPoint) const; ///< Velocity of point inPoint (in world space, e.g. on the surface of the body) of the body
 
 	/// Set the complete motion state of a body.
 	/// Note that the linear velocity is the velocity of the center of mass, which may not coincide with the position of your object, to correct for this: \f$VelocityCOM = Velocity - AngularVelocity \times ShapeCOM\f$
-	void						SetPositionRotationAndVelocity(const BodyID &inBodyID, Vec3Arg inPosition, QuatArg inRotation, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity);
+	void						SetPositionRotationAndVelocity(const BodyID &inBodyID, RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity);
 
 	///@name Add forces to the body
 	///@{
 	void						AddForce(const BodyID &inBodyID, Vec3Arg inForce); ///< See Body::AddForce
-	void						AddForce(const BodyID &inBodyID, Vec3Arg inForce, Vec3Arg inPoint); ///< Applied at inPoint
+	void						AddForce(const BodyID &inBodyID, Vec3Arg inForce, RVec3Arg inPoint); ///< Applied at inPoint
 	void						AddTorque(const BodyID &inBodyID, Vec3Arg inTorque); ///< See Body::AddTorque
 	void						AddForceAndTorque(const BodyID &inBodyID, Vec3Arg inForce, Vec3Arg inTorque); ///< A combination of Body::AddForce and Body::AddTorque
 	///@}
@@ -188,7 +188,7 @@ public:
 	///@name Add an impulse to the body
 	///@{
 	void						AddImpulse(const BodyID &inBodyID, Vec3Arg inImpulse); ///< Applied at center of mass
-	void						AddImpulse(const BodyID &inBodyID, Vec3Arg inImpulse, Vec3Arg inPoint); ///< Applied at inPoint
+	void						AddImpulse(const BodyID &inBodyID, Vec3Arg inImpulse, RVec3Arg inPoint); ///< Applied at inPoint
 	void						AddAngularImpulse(const BodyID &inBodyID, Vec3Arg inAngularImpulse);
 	///@}
 

Jolt/Physics/Body/BodyManager.cpp

@@ -847,7 +847,7 @@ void BodyManager::Draw(const DrawSettings &inDrawSettings, const PhysicsSettings
 			// Draw world space linear and angular velocity
 			if (inDrawSettings.mDrawVelocity)
 			{
-				Vec3 pos = body->GetCenterOfMassPosition();
+				RVec3 pos = body->GetCenterOfMassPosition();
 				inRenderer->DrawArrow(pos, pos + body->GetLinearVelocity(), Color::sGreen, 0.1f);
 				inRenderer->DrawArrow(pos, pos + body->GetAngularVelocity(), Color::sRed, 0.1f);
 			}
@@ -880,7 +880,7 @@ void BodyManager::Draw(const DrawSettings &inDrawSettings, const PhysicsSettings
 				Color sleep_color = Color(0, 255 - g, g);
 				inRenderer->DrawText3D(body->GetCenterOfMassPosition(), text, sleep_color, 0.2f);
 				for (int i = 0; i < 3; ++i)
-					inRenderer->DrawWireSphere(body->mMotionProperties->mSleepTestSpheres[i].GetCenter(), body->mMotionProperties->mSleepTestSpheres[i].GetRadius(), sleep_color);
+					inRenderer->DrawWireSphere(JPH_IF_DOUBLE_PRECISION(body->mMotionProperties->GetSleepTestOffset() +) body->mMotionProperties->mSleepTestSpheres[i].GetCenter(), body->mMotionProperties->mSleepTestSpheres[i].GetRadius(), sleep_color);
 			}
 		}
 

Jolt/Physics/Body/MotionProperties.cpp

@@ -20,6 +20,9 @@ void MotionProperties::SaveState(StateRecorder &inStream) const
 	inStream.Write(mMaxLinearVelocity);
 	inStream.Write(mMaxAngularVelocity);
 	inStream.Write(mGravityFactor);
+#ifdef JPH_DOUBLE_PRECISION
+	inStream.Write(mSleepTestOffset);
+#endif // JPH_DOUBLE_PRECISION
 	inStream.Write(mSleepTestSpheres);
 	inStream.Write(mSleepTestTimer);
 	inStream.Write(mMotionQuality);
@@ -37,6 +40,9 @@ void MotionProperties::RestoreState(StateRecorder &inStream)
 	inStream.Read(mMaxLinearVelocity);
 	inStream.Read(mMaxAngularVelocity);
 	inStream.Read(mGravityFactor);
+#ifdef JPH_DOUBLE_PRECISION
+	inStream.Read(mSleepTestOffset);
+#endif // JPH_DOUBLE_PRECISION
 	inStream.Read(mSleepTestSpheres);
 	inStream.Read(mSleepTestTimer);
 	inStream.Read(mMotionQuality);

Jolt/Physics/Body/MotionProperties.h

@@ -141,8 +141,12 @@ public:
 	/// Access to the index in the active bodies array
 	uint32					GetIndexInActiveBodiesInternal() const							{ return mIndexInActiveBodies; }
 
+#ifdef JPH_DOUBLE_PRECISION
+	inline DVec3			GetSleepTestOffset() const										{ return DVec3::sLoadDouble3Unsafe(mSleepTestOffset); }
+#endif // JPH_DOUBLE_PRECISION
+
 	/// Reset spheres to center around inPoints with radius 0
-	inline void				ResetSleepTestSpheres(const Vec3 *inPoints);
+	inline void				ResetSleepTestSpheres(const RVec3 *inPoints);
 
 	/// Saving state for replay
 	void					SaveState(StateRecorder &inStream) const;
@@ -179,7 +183,10 @@ private:
 	bool					mAllowSleeping;													///< If this body can go to sleep
 
 	// 3rd cache line (least frequently used)
-	// 4 byte aligned
+	// 4 byte aligned (or 8 byte if running in double precision)
+#ifdef JPH_DOUBLE_PRECISION
+	Double3					mSleepTestOffset;												///< mSleepTestSpheres are relative to this offset to prevent floating point inaccuracies. Warning: Loaded using sLoadDouble3Unsafe which will read 8 extra bytes.
+#endif // JPH_DOUBLE_PRECISION
 	Sphere					mSleepTestSpheres[3];											///< Measure motion for 3 points on the body to see if it is resting: COM, COM + largest bounding box axis, COM + second largest bounding box axis
 	float					mSleepTestTimer;												///< How long this body has been within the movement tolerance
 

Jolt/Physics/Body/MotionProperties.inl

@@ -117,10 +117,21 @@ void MotionProperties::ApplyForceTorqueAndDragInternal(QuatArg inBodyRotation, V
 	ClampAngularVelocity();
 }
 
-void MotionProperties::ResetSleepTestSpheres(const Vec3 *inPoints)
+void MotionProperties::ResetSleepTestSpheres(const RVec3 *inPoints)
 {
+#ifdef JPH_DOUBLE_PRECISION
+	// Make spheres relative to the first point and initialize them to zero radius
+	DVec3 offset = inPoints[0];
+	offset.StoreDouble3(&mSleepTestOffset);
+	mSleepTestSpheres[0] = Sphere(Vec3::sZero(), 0.0f);
+	for (int i = 1; i < 3; ++i)
+		mSleepTestSpheres[i] = Sphere(Vec3(inPoints[i] - offset), 0.0f);
+#else
+	// Initialize the spheres to zero radius around the supplied points
 	for (int i = 0; i < 3; ++i)
 		mSleepTestSpheres[i] = Sphere(inPoints[i], 0.0f);
+#endif
+
 	mSleepTestTimer = 0.0f;
 }
 

Jolt/Physics/Character/Character.cpp

@@ -27,7 +27,7 @@ static inline const NarrowPhaseQuery &sGetNarrowPhaseQuery(const PhysicsSystem *
 	return inLockBodies? inSystem->GetNarrowPhaseQuery() : inSystem->GetNarrowPhaseQueryNoLock();
 }
 
-Character::Character(const CharacterSettings *inSettings, Vec3Arg inPosition, QuatArg inRotation, uint64 inUserData, PhysicsSystem *inSystem) :
+Character::Character(const CharacterSettings *inSettings, RVec3Arg inPosition, QuatArg inRotation, uint64 inUserData, PhysicsSystem *inSystem) :
 	CharacterBase(inSettings, inSystem),
 	mLayer(inSettings->mLayer)
 {
@@ -68,7 +68,7 @@ void Character::Activate(bool inLockBodies)
 	sGetBodyInterface(mSystem, inLockBodies).ActivateBody(mBodyID);
 }
 
-void Character::CheckCollision(Mat44Arg inCenterOfMassTransform, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, CollideShapeCollector &ioCollector, bool inLockBodies) const
+void Character::CheckCollision(RMat44Arg inCenterOfMassTransform, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, bool inLockBodies) const
 {
 	// Create query broadphase layer filter
 	DefaultBroadPhaseLayerFilter broadphase_layer_filter = mSystem->GetDefaultBroadPhaseLayerFilter(mLayer);
@@ -86,21 +86,21 @@ void Character::CheckCollision(Mat44Arg inCenterOfMassTransform, Vec3Arg inMovem
 	settings.mActiveEdgeMovementDirection = inMovementDirection;
 	settings.mBackFaceMode = EBackFaceMode::IgnoreBackFaces;
 
-	sGetNarrowPhaseQuery(mSystem, inLockBodies).CollideShape(inShape, Vec3::sReplicate(1.0f), inCenterOfMassTransform, settings, ioCollector, broadphase_layer_filter, object_layer_filter, body_filter);
+	sGetNarrowPhaseQuery(mSystem, inLockBodies).CollideShape(inShape, Vec3::sReplicate(1.0f), inCenterOfMassTransform, settings, inBaseOffset, ioCollector, broadphase_layer_filter, object_layer_filter, body_filter);
 }
 
-void Character::CheckCollision(Vec3Arg inPosition, QuatArg inRotation, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, CollideShapeCollector &ioCollector, bool inLockBodies) const
+void Character::CheckCollision(RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, bool inLockBodies) const
 {
 	// Calculate center of mass transform
-	Mat44 center_of_mass = Mat44::sRotationTranslation(inRotation, inPosition).PreTranslated(inShape->GetCenterOfMass());
+	RMat44 center_of_mass = RMat44::sRotationTranslation(inRotation, inPosition).PreTranslated(inShape->GetCenterOfMass());
 
-	CheckCollision(center_of_mass, inMovementDirection, inMaxSeparationDistance, inShape, ioCollector, inLockBodies);
+	CheckCollision(center_of_mass, inMovementDirection, inMaxSeparationDistance, inShape, inBaseOffset, ioCollector, inLockBodies);
 }
 
-void Character::CheckCollision(const Shape *inShape, float inMaxSeparationDistance, CollideShapeCollector &ioCollector, bool inLockBodies) const
+void Character::CheckCollision(const Shape *inShape, float inMaxSeparationDistance, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, bool inLockBodies) const
 {
 	// Determine position and velocity of body
-	Mat44 query_transform;
+	RMat44 query_transform;
 	Vec3 velocity;
 	{
 		BodyLockRead lock(sGetBodyLockInterface(mSystem, inLockBodies), mBodyID);
@@ -114,13 +114,13 @@ void Character::CheckCollision(const Shape *inShape, float inMaxSeparationDistan
 		velocity = body.GetLinearVelocity();
 	}
 
-	CheckCollision(query_transform, velocity, inMaxSeparationDistance, inShape, ioCollector, inLockBodies);
+	CheckCollision(query_transform, velocity, inMaxSeparationDistance, inShape, inBaseOffset, ioCollector, inLockBodies);
 }
 
 void Character::PostSimulation(float inMaxSeparationDistance, bool inLockBodies)
 {
 	// Get character position, rotation and velocity
-	Vec3 char_pos;
+	RVec3 char_pos;
 	Quat char_rot;
 	Vec3 char_vel;
 	{
@@ -138,7 +138,7 @@ void Character::PostSimulation(float inMaxSeparationDistance, bool inLockBodies)
 	{
 	public:
 		// Constructor
-		explicit			MyCollector(Vec3Arg inUp) : mUp(inUp) { }
+		explicit			MyCollector(Vec3Arg inUp, RVec3 inBaseOffset) : mBaseOffset(inBaseOffset), mUp(inUp) { }
 
 		// See: CollectorType::AddHit
 		virtual void		AddHit(const CollideShapeResult &inResult) override
@@ -149,7 +149,7 @@ void Character::PostSimulation(float inMaxSeparationDistance, bool inLockBodies)
 			{
 				mGroundBodyID = inResult.mBodyID2;
 				mGroundBodySubShapeID = inResult.mSubShapeID2;
-				mGroundPosition = inResult.mContactPointOn2;
+				mGroundPosition = mBaseOffset + inResult.mContactPointOn2;
 				mGroundNormal = normal;
 				mBestDot = dot;
 			}
@@ -157,17 +157,18 @@ void Character::PostSimulation(float inMaxSeparationDistance, bool inLockBodies)
 
 		BodyID				mGroundBodyID;
 		SubShapeID			mGroundBodySubShapeID;
-		Vec3				mGroundPosition = Vec3::sZero();
+		RVec3				mGroundPosition = RVec3::sZero();
 		Vec3				mGroundNormal = Vec3::sZero();
 
 	private:
-		float				mBestDot = -FLT_MAX;
+		RVec3				mBaseOffset;
 		Vec3				mUp;
+		float				mBestDot = -FLT_MAX;
 	};
 
 	// Collide shape
-	MyCollector collector(mUp);
-	CheckCollision(char_pos, char_rot, char_vel, inMaxSeparationDistance, mShape, collector, inLockBodies);
+	MyCollector collector(mUp, char_pos);
+	CheckCollision(char_pos, char_rot, char_vel, inMaxSeparationDistance, mShape, char_pos, collector, inLockBodies);
 
 	// Copy results
 	mGroundBodyID = collector.mGroundBodyID;
@@ -182,8 +183,8 @@ void Character::PostSimulation(float inMaxSeparationDistance, bool inLockBodies)
 		const Body &body = lock.GetBody();
 
 		// Update ground state
-		Mat44 inv_transform = Mat44::sInverseRotationTranslation(char_rot, char_pos);
-		if (mSupportingVolume.SignedDistance(inv_transform * mGroundPosition) > 0.0f)
+		RMat44 inv_transform = RMat44::sInverseRotationTranslation(char_rot, char_pos);
+		if (mSupportingVolume.SignedDistance(Vec3(inv_transform * mGroundPosition)) > 0.0f)
 			mGroundState = EGroundState::NotSupported;
 		else if (IsSlopeTooSteep(mGroundNormal))
 			mGroundState = EGroundState::OnSteepGround;
@@ -229,22 +230,22 @@ void Character::AddImpulse(Vec3Arg inImpulse, bool inLockBodies)
 	sGetBodyInterface(mSystem, inLockBodies).AddImpulse(mBodyID, inImpulse);
 }
 
-void Character::GetPositionAndRotation(Vec3 &outPosition, Quat &outRotation, bool inLockBodies) const
+void Character::GetPositionAndRotation(RVec3 &outPosition, Quat &outRotation, bool inLockBodies) const
 {
 	sGetBodyInterface(mSystem, inLockBodies).GetPositionAndRotation(mBodyID, outPosition, outRotation);
 }
 
-void Character::SetPositionAndRotation(Vec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode, bool inLockBodies) const
+void Character::SetPositionAndRotation(RVec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode, bool inLockBodies) const
 {
 	sGetBodyInterface(mSystem, inLockBodies).SetPositionAndRotation(mBodyID, inPosition, inRotation, inActivationMode);
 }
 
-Vec3 Character::GetPosition(bool inLockBodies) const
+RVec3 Character::GetPosition(bool inLockBodies) const
 {
 	return sGetBodyInterface(mSystem, inLockBodies).GetPosition(mBodyID);
 }
 
-void Character::SetPosition(Vec3Arg inPosition, EActivation inActivationMode, bool inLockBodies) 
+void Character::SetPosition(RVec3Arg inPosition, EActivation inActivationMode, bool inLockBodies) 
 {
 	sGetBodyInterface(mSystem, inLockBodies).SetPosition(mBodyID, inPosition, inActivationMode);
 }
@@ -259,12 +260,12 @@ void Character::SetRotation(QuatArg inRotation, EActivation inActivationMode, bo
 	sGetBodyInterface(mSystem, inLockBodies).SetRotation(mBodyID, inRotation, inActivationMode);
 }
 
-Vec3 Character::GetCenterOfMassPosition(bool inLockBodies) const
+RVec3 Character::GetCenterOfMassPosition(bool inLockBodies) const
 {
 	return sGetBodyInterface(mSystem, inLockBodies).GetCenterOfMassPosition(mBodyID);
 }
 
-Mat44 Character::GetWorldTransform(bool inLockBodies) const
+RMat44 Character::GetWorldTransform(bool inLockBodies) const
 {
 	return sGetBodyInterface(mSystem, inLockBodies).GetWorldTransform(mBodyID);
 }
@@ -302,8 +303,9 @@ bool Character::SetShape(const Shape *inShape, float inMaxPenetrationDepth, bool
 		};
 
 		// Test if anything is in the way of switching
+		RVec3 char_pos = GetPosition(inLockBodies);
 		MyCollector collector(inMaxPenetrationDepth);
-		CheckCollision(inShape, 0.0f, collector, inLockBodies);
+		CheckCollision(inShape, 0.0f, char_pos, collector, inLockBodies);
 		if (collector.mHadCollision)
 			return false;
 	}

Jolt/Physics/Character/Character.h

@@ -42,7 +42,7 @@ public:
 	/// @param inRotation Initial rotation for the character (usually only around Y)
 	/// @param inUserData Application specific value
 	/// @param inSystem Physics system that this character will be added to later
-										Character(const CharacterSettings *inSettings, Vec3Arg inPosition, QuatArg inRotation, uint64 inUserData, PhysicsSystem *inSystem);
+										Character(const CharacterSettings *inSettings, RVec3Arg inPosition, QuatArg inRotation, uint64 inUserData, PhysicsSystem *inSystem);
 
 	/// Destructor
 	virtual								~Character() override;
@@ -80,16 +80,16 @@ public:
 	BodyID								GetBodyID() const										{ return mBodyID; }
 
 	/// Get position / rotation of the body
-	void								GetPositionAndRotation(Vec3 &outPosition, Quat &outRotation, bool inLockBodies = true) const;
+	void								GetPositionAndRotation(RVec3 &outPosition, Quat &outRotation, bool inLockBodies = true) const;
 
 	/// Set the position / rotation of the body, optionally activating it.
-	void								SetPositionAndRotation(Vec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode = EActivation::Activate, bool inLockBodies = true) const;
+	void								SetPositionAndRotation(RVec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode = EActivation::Activate, bool inLockBodies = true) const;
 
 	/// Get the position of the character
-	Vec3								GetPosition(bool inLockBodies = true) const;
+	RVec3								GetPosition(bool inLockBodies = true) const;
 
 	/// Set the position of the character, optionally activating it.
-	void								SetPosition(Vec3Arg inPostion, EActivation inActivationMode = EActivation::Activate, bool inLockBodies = true);
+	void								SetPosition(RVec3Arg inPostion, EActivation inActivationMode = EActivation::Activate, bool inLockBodies = true);
 
 	/// Get the rotation of the character
 	Quat								GetRotation(bool inLockBodies = true) const;
@@ -98,10 +98,10 @@ public:
 	void								SetRotation(QuatArg inRotation, EActivation inActivationMode = EActivation::Activate, bool inLockBodies = true);
 
 	/// Position of the center of mass of the underlying rigid body
-	Vec3								GetCenterOfMassPosition(bool inLockBodies = true) const;
+	RVec3								GetCenterOfMassPosition(bool inLockBodies = true) const;
 
 	/// Calculate the world transform of the character
-	Mat44								GetWorldTransform(bool inLockBodies = true) const;
+	RMat44								GetWorldTransform(bool inLockBodies = true) const;
 
 	/// Update the layer of the character
 	void								SetLayer(ObjectLayer inLayer, bool inLockBodies = true);
@@ -116,16 +116,17 @@ public:
 	/// @param inMovementDirection A hint in which direction the character is moving, will be used to calculate a proper normal.
 	/// @param inMaxSeparationDistance How much distance around the character you want to report contacts in (can be 0 to match the character exactly).
 	/// @param inShape Shape to test collision with.
+	/// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. GetPosition() since floats are most accurate near the origin
 	/// @param ioCollector Collision collector that receives the collision results.
 	/// @param inLockBodies If the collision query should use the locking body interface (true) or the non locking body interface (false)
-	void								CheckCollision(Vec3Arg inPosition, QuatArg inRotation, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, CollideShapeCollector &ioCollector, bool inLockBodies = true) const;
+	void								CheckCollision(RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, bool inLockBodies = true) const;
 
 private:
 	/// Check collisions between inShape and the world using the center of mass transform
-	void								CheckCollision(Mat44Arg inCenterOfMassTransform, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, CollideShapeCollector &ioCollector, bool inLockBodies) const;
+	void								CheckCollision(RMat44Arg inCenterOfMassTransform, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, bool inLockBodies) const;
 
 	/// Check collisions between inShape and the world using the current position / rotation of the character
-	void								CheckCollision(const Shape *inShape, float inMaxSeparationDistance, CollideShapeCollector &ioCollector, bool inLockBodies) const;
+	void								CheckCollision(const Shape *inShape, float inMaxSeparationDistance, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, bool inLockBodies) const;
 
 	/// The body of this character
 	BodyID								mBodyID;

Jolt/Physics/Character/CharacterBase.h

@@ -88,7 +88,7 @@ public:
 	bool								IsSupported() const										{ return mGroundState == EGroundState::OnGround || mGroundState == EGroundState::OnSteepGround; }
 
 	/// Get the contact point with the ground
-	Vec3 								GetGroundPosition() const								{ return mGroundPosition; }
+	RVec3 								GetGroundPosition() const								{ return mGroundPosition; }
 
 	/// Get the contact normal with the ground
 	Vec3	 							GetGroundNormal() const									{ return mGroundNormal; }
@@ -135,7 +135,7 @@ protected:
 	EGroundState						mGroundState = EGroundState::InAir;
 	BodyID								mGroundBodyID;
 	SubShapeID							mGroundBodySubShapeID;
-	Vec3								mGroundPosition = Vec3::sZero();
+	RVec3								mGroundPosition = RVec3::sZero();
 	Vec3								mGroundNormal = Vec3::sZero();
 	Vec3								mGroundVelocity = Vec3::sZero();
 	RefConst<PhysicsMaterial>			mGroundMaterial = PhysicsMaterial::sDefault;

Jolt/Physics/Character/CharacterVirtual.cpp

@@ -18,7 +18,7 @@
 
 JPH_NAMESPACE_BEGIN
 
-CharacterVirtual::CharacterVirtual(const CharacterVirtualSettings *inSettings, Vec3Arg inPosition, QuatArg inRotation, PhysicsSystem *inSystem) :
+CharacterVirtual::CharacterVirtual(const CharacterVirtualSettings *inSettings, RVec3Arg inPosition, QuatArg inRotation, PhysicsSystem *inSystem) :
 	CharacterBase(inSettings, inSystem),
 	mPredictiveContactDistance(inSettings->mPredictiveContactDistance),
 	mMaxCollisionIterations(inSettings->mMaxCollisionIterations),
@@ -37,12 +37,12 @@ CharacterVirtual::CharacterVirtual(const CharacterVirtualSettings *inSettings, V
 }
 
 template <class taCollector>
-void CharacterVirtual::sFillContactProperties(Contact &outContact, const Body &inBody, Vec3Arg inUp, const taCollector &inCollector, const CollideShapeResult &inResult)
+void CharacterVirtual::sFillContactProperties(Contact &outContact, const Body &inBody, Vec3Arg inUp, RVec3Arg inBaseOffset, const taCollector &inCollector, const CollideShapeResult &inResult)
 {
-	outContact.mPosition = inResult.mContactPointOn2;	
-	outContact.mLinearVelocity = inBody.GetPointVelocity(inResult.mContactPointOn2);
+	outContact.mPosition = inBaseOffset + inResult.mContactPointOn2;
+	outContact.mLinearVelocity = inBody.GetPointVelocity(outContact.mPosition);
 	outContact.mContactNormal = -inResult.mPenetrationAxis.NormalizedOr(Vec3::sZero());
-	outContact.mSurfaceNormal = inCollector.GetContext()->GetWorldSpaceSurfaceNormal(inResult.mSubShapeID2, inResult.mContactPointOn2);
+	outContact.mSurfaceNormal = inCollector.GetContext()->GetWorldSpaceSurfaceNormal(inResult.mSubShapeID2, outContact.mPosition);
 	if (outContact.mContactNormal.Dot(outContact.mSurfaceNormal) < 0.0f)
 		outContact.mSurfaceNormal = -outContact.mSurfaceNormal; // Flip surface normal if we're hitting a back face
 	if (outContact.mContactNormal.Dot(inUp) > outContact.mSurfaceNormal.Dot(inUp))
@@ -64,7 +64,7 @@ void CharacterVirtual::ContactCollector::AddHit(const CollideShapeResult &inResu
 
 		mContacts.emplace_back();
 		Contact &contact = mContacts.back();
-		sFillContactProperties(contact, body, mUp, *this, inResult);
+		sFillContactProperties(contact, body, mUp, mBaseOffset, *this, inResult);
 		contact.mFraction = 0.0f;
 
 		// Protection from excess of contact points
@@ -90,7 +90,7 @@ void CharacterVirtual::ContactCastCollector::AddHit(const ShapeCastResult &inRes
 
 			mContacts.emplace_back();
 			Contact &contact = mContacts.back();
-			sFillContactProperties(contact, body, mUp, *this, inResult);
+			sFillContactProperties(contact, body, mUp, mBaseOffset, *this, inResult);
 			contact.mFraction = inResult.mFraction;
 
 			// Protection from excess of contact points
@@ -100,10 +100,10 @@ void CharacterVirtual::ContactCastCollector::AddHit(const ShapeCastResult &inRes
 	}
 }
 
-void CharacterVirtual::CheckCollision(Vec3Arg inPosition, QuatArg inRotation, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, CollideShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter) const
+void CharacterVirtual::CheckCollision(RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter) const
 {
 	// Query shape transform
-	Mat44 transform = GetCenterOfMassTransform(inPosition, inRotation, inShape);
+	RMat44 transform = GetCenterOfMassTransform(inPosition, inRotation, inShape);
 
 	// Settings for collide shape
 	CollideShapeSettings settings;
@@ -113,17 +113,17 @@ void CharacterVirtual::CheckCollision(Vec3Arg inPosition, QuatArg inRotation, Ve
 	settings.mMaxSeparationDistance = mCharacterPadding + inMaxSeparationDistance;
 
 	// Collide shape
-	mSystem->GetNarrowPhaseQuery().CollideShape(inShape, Vec3::sReplicate(1.0f), transform, settings, ioCollector, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter);
+	mSystem->GetNarrowPhaseQuery().CollideShape(inShape, Vec3::sReplicate(1.0f), transform, settings, inBaseOffset, ioCollector, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter);
 }
 
-void CharacterVirtual::GetContactsAtPosition(Vec3Arg inPosition, Vec3Arg inMovementDirection, const Shape *inShape, TempContactList &outContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter) const
+void CharacterVirtual::GetContactsAtPosition(RVec3Arg inPosition, Vec3Arg inMovementDirection, const Shape *inShape, TempContactList &outContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter) const
 {
 	// Remove previous results
 	outContacts.clear();
 
 	// Collide shape
-	ContactCollector collector(mSystem, mMaxNumHits, mUp, outContacts);
-	CheckCollision(inPosition, mRotation, inMovementDirection, mPredictiveContactDistance, inShape, collector, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter);
+	ContactCollector collector(mSystem, mMaxNumHits, mUp, mPosition, outContacts);
+	CheckCollision(inPosition, mRotation, inMovementDirection, mPredictiveContactDistance, inShape, mPosition, collector, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter);
 
 	// Reduce distance to contact by padding to ensure we stay away from the object by a little margin
 	// (this will make collision detection cheaper - especially for sweep tests as they won't hit the surface if we're properly sliding)
@@ -204,7 +204,7 @@ inline static bool sCorrectFractionForCharacterPadding(const Shape *inShape, Mat
 	}
 }
 
-bool CharacterVirtual::GetFirstContactForSweep(Vec3Arg inPosition, Vec3Arg inDisplacement, Contact &outContact, const IgnoredContactList &inIgnoredContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, TempAllocator &inAllocator) const
+bool CharacterVirtual::GetFirstContactForSweep(RVec3Arg inPosition, Vec3Arg inDisplacement, Contact &outContact, const IgnoredContactList &inIgnoredContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, TempAllocator &inAllocator) const
 {
 	// Too small distance -> skip checking
 	float displacement_len_sq = inDisplacement.LengthSq();
@@ -212,7 +212,7 @@ bool CharacterVirtual::GetFirstContactForSweep(Vec3Arg inPosition, Vec3Arg inDis
 		return false;
 
 	// Calculate start transform
-	Mat44 start = GetCenterOfMassTransform(inPosition, mRotation, mShape);
+	RMat44 start = GetCenterOfMassTransform(inPosition, mRotation, mShape);
 
 	// Settings for the cast
 	ShapeCastSettings settings;
@@ -225,9 +225,9 @@ bool CharacterVirtual::GetFirstContactForSweep(Vec3Arg inPosition, Vec3Arg inDis
 	// Cast shape
 	TempContactList contacts(inAllocator);
 	contacts.reserve(mMaxNumHits);
-	ContactCastCollector collector(mSystem, inDisplacement, mMaxNumHits, mUp, inIgnoredContacts, contacts);
-	ShapeCast shape_cast(mShape, Vec3::sReplicate(1.0f), start, inDisplacement);
-	mSystem->GetNarrowPhaseQuery().CastShape(shape_cast, settings, collector, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter);
+	ContactCastCollector collector(mSystem, inDisplacement, mMaxNumHits, mUp, inIgnoredContacts, start.GetTranslation(), contacts);
+	RShapeCast shape_cast(mShape, Vec3::sReplicate(1.0f), start, inDisplacement);
+	mSystem->GetNarrowPhaseQuery().CastShape(shape_cast, settings, start.GetTranslation(), collector, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter);
 	if (contacts.empty())
 		return false;
 
@@ -250,7 +250,7 @@ bool CharacterVirtual::GetFirstContactForSweep(Vec3Arg inPosition, Vec3Arg inDis
 	// Fetch the face we're colliding with
 	TransformedShape ts = mSystem->GetBodyInterface().GetTransformedShape(outContact.mBodyB);
 	Shape::SupportingFace face;
-	ts.GetSupportingFace(outContact.mSubShapeIDB, -outContact.mContactNormal, face);
+	ts.GetSupportingFace(outContact.mSubShapeIDB, -outContact.mContactNormal, start.GetTranslation(), face);
 
 	bool corrected = false;
 	if (face.size() >= 2)
@@ -260,7 +260,7 @@ bool CharacterVirtual::GetFirstContactForSweep(Vec3Arg inPosition, Vec3Arg inDis
 		AddConvexRadius add_cvx(polygon, mCharacterPadding);
 
 		// Correct fraction to hit this inflated face instead of the inner shape
-		corrected = sCorrectFractionForCharacterPadding(mShape, start, inDisplacement, add_cvx, outContact.mFraction);
+		corrected = sCorrectFractionForCharacterPadding(mShape, start.GetRotation(), inDisplacement, add_cvx, outContact.mFraction);
 	}
 	if (!corrected)
 	{
@@ -347,12 +347,12 @@ bool CharacterVirtual::HandleContact(Vec3Arg inVelocity, Constraint &ioConstrain
 
 	// Determine mass properties of the body we're colliding with
 	const MotionProperties *motion_properties = body.GetMotionProperties();
-	Vec3 center_of_mass = body.GetCenterOfMassPosition();
+	RVec3 center_of_mass = body.GetCenterOfMassPosition();
 	Mat44 inverse_inertia = body.GetInverseInertia();
 	float inverse_mass = motion_properties->GetInverseMass();
 
 	// Calculate the inverse of the mass of body B as seen at the contact point in the direction of the contact normal
-	Vec3 jacobian = (contact.mPosition - center_of_mass).Cross(contact.mContactNormal);
+	Vec3 jacobian = Vec3(contact.mPosition - center_of_mass).Cross(contact.mContactNormal);
 	float inv_effective_mass = inverse_inertia.Multiply3x3(jacobian).Dot(jacobian) + inverse_mass;
 
 	// Impulse P = M dv
@@ -581,7 +581,7 @@ void CharacterVirtual::SolveConstraints(Vec3Arg inVelocity, float inDeltaTime, f
 		if (inDrawConstraints)
 		{
 			// Calculate where to draw
-			Vec3 offset = mPosition + Vec3(0, 0, 2.5f * (iteration + 1));
+			RVec3 offset = mPosition + Vec3(0, 0, 2.5f * (iteration + 1));
 
 			// Draw constraint plane
 			DebugRenderer::sInstance->DrawPlane(offset, constraint->mPlane.GetNormal(), Color::sCyan, 1.0f);
@@ -628,7 +628,7 @@ void CharacterVirtual::UpdateSupportingContact(bool inSkipContactVelocityCheck,
 								&& (inSkipContactVelocityCheck || c.mSurfaceNormal.Dot(mLinearVelocity - c.mLinearVelocity) <= 0.0f);
 
 	// Calculate transform that takes us to character local space
-	Mat44 inv_transform = Mat44::sInverseRotationTranslation(mRotation, mPosition);
+	RMat44 inv_transform = RMat44::sInverseRotationTranslation(mRotation, mPosition);
 
 	// Determine if we're supported or not
 	int num_supported = 0;
@@ -654,7 +654,7 @@ void CharacterVirtual::UpdateSupportingContact(bool inSkipContactVelocityCheck,
 			}
 
 			// If this contact is in front of our plane, we cannot be supported by it
-			if (mSupportingVolume.SignedDistance(inv_transform * c.mPosition) > 0.0f)
+			if (mSupportingVolume.SignedDistance(Vec3(inv_transform * c.mPosition)) > 0.0f)
 				continue;
 
 			// Find the contact with the normal that is pointing most upwards and store it
@@ -687,7 +687,8 @@ void CharacterVirtual::UpdateSupportingContact(bool inSkipContactVelocityCheck,
 					// so if you just take point velocity * delta time you get an error that accumulates over time
 
 					// Determine center of mass and angular velocity
-					Vec3 angular_velocity, com;
+					Vec3 angular_velocity;
+					RVec3 com;
 					{
 						BodyLockRead lock(mSystem->GetBodyLockInterface(), c.mBodyB);
 						if (lock.SucceededAndIsInBroadPhase())
@@ -703,7 +704,7 @@ void CharacterVirtual::UpdateSupportingContact(bool inSkipContactVelocityCheck,
 						else
 						{
 							angular_velocity = Vec3::sZero();
-							com = Vec3::sZero();
+							com = RVec3::sZero();
 						}
 					}
 
@@ -717,10 +718,10 @@ void CharacterVirtual::UpdateSupportingContact(bool inSkipContactVelocityCheck,
 						Quat rotation = Quat::sRotation(angular_velocity / angular_velocity_len, angular_velocity_len * mLastDeltaTime);
 
 						// Calculate where the new contact position will be
-						Vec3 new_position = com + rotation * (mPosition - com);
+						RVec3 new_position = com + rotation * Vec3(mPosition - com);
 
 						// Calculate the velocity
-						avg_velocity += (new_position - mPosition) / mLastDeltaTime;
+						avg_velocity += Vec3(new_position - mPosition) / mLastDeltaTime;
 					}
 				}
 			}
@@ -759,7 +760,7 @@ void CharacterVirtual::UpdateSupportingContact(bool inSkipContactVelocityCheck,
 	{
 		mGroundBodyID = BodyID();
 		mGroundBodySubShapeID = SubShapeID();
-		mGroundPosition = Vec3::sZero();
+		mGroundPosition = RVec3::sZero();
 		mGroundMaterial = PhysicsMaterial::sDefault;
 		mGroundUserData = 0;
 	}
@@ -808,7 +809,7 @@ void CharacterVirtual::StoreActiveContacts(const TempContactList &inContacts, Te
 	UpdateSupportingContact(true, inAllocator);
 }
 
-void CharacterVirtual::MoveShape(Vec3 &ioPosition, Vec3Arg inVelocity, float inDeltaTime, ContactList *outActiveContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, TempAllocator &inAllocator
+void CharacterVirtual::MoveShape(RVec3 &ioPosition, Vec3Arg inVelocity, float inDeltaTime, ContactList *outActiveContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, TempAllocator &inAllocator
 #ifdef JPH_DEBUG_RENDERER
 	, bool inDrawConstraints
 #endif // JPH_DEBUG_RENDERER
@@ -953,7 +954,7 @@ void CharacterVirtual::RefreshContacts(const BroadPhaseLayerFilter &inBroadPhase
 	StoreActiveContacts(contacts, inAllocator);
 }
 
-void CharacterVirtual::MoveToContact(Vec3Arg inPosition, const Contact &inContact, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, TempAllocator &inAllocator)
+void CharacterVirtual::MoveToContact(RVec3Arg inPosition, const Contact &inContact, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, TempAllocator &inAllocator)
 {
 	// Set the new position
 	SetPosition(inPosition);
@@ -1052,7 +1053,7 @@ bool CharacterVirtual::WalkStairs(float inDeltaTime, Vec3Arg inStepUp, Vec3Arg i
 		// Limit up movement to the first contact point
 		up *= contact.mFraction;
 	}
-	Vec3 up_position = mPosition + up;
+	RVec3 up_position = mPosition + up;
 
 #ifdef JPH_DEBUG_RENDERER
 	// Draw sweep up
@@ -1061,9 +1062,9 @@ bool CharacterVirtual::WalkStairs(float inDeltaTime, Vec3Arg inStepUp, Vec3Arg i
 #endif // JPH_DEBUG_RENDERER
 
 	// Horizontal movement
-	Vec3 new_position = up_position;
+	RVec3 new_position = up_position;
 	MoveShape(new_position, inStepForward / inDeltaTime, inDeltaTime, nullptr, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inAllocator);
-	float horizontal_movement_sq = (new_position - up_position).LengthSq();
+	float horizontal_movement_sq = Vec3(new_position - up_position).LengthSq();
 	if (horizontal_movement_sq < 1.0e-8f)
 		return false; // No movement, cancel
 
@@ -1084,7 +1085,7 @@ bool CharacterVirtual::WalkStairs(float inDeltaTime, Vec3Arg inStepUp, Vec3Arg i
 	// Draw sweep down
 	if (sDrawWalkStairs)
 	{
-		Vec3 debug_pos = new_position + contact.mFraction * down; 
+		RVec3 debug_pos = new_position + contact.mFraction * down; 
 		DebugRenderer::sInstance->DrawArrow(new_position, debug_pos, Color::sWhite, 0.01f);
 		DebugRenderer::sInstance->DrawArrow(contact.mPosition, contact.mPosition + contact.mSurfaceNormal, Color::sWhite, 0.01f);
 		mShape->Draw(DebugRenderer::sInstance, GetCenterOfMassTransform(debug_pos, mRotation, mShape), Vec3::sReplicate(1.0f), Color::sWhite, false, true);
@@ -1101,9 +1102,9 @@ bool CharacterVirtual::WalkStairs(float inDeltaTime, Vec3Arg inStepUp, Vec3Arg i
 		// Delta time may be very small, so it may be that we hit the edge of a step and the normal is too horizontal.
 		// In order to judge if the floor is flat further along the sweep, we test again for a floor at inStepForwardTest
 		// and check if the normal is valid there.
-		Vec3 test_position = up_position;
+		RVec3 test_position = up_position;
 		MoveShape(test_position, inStepForwardTest / inDeltaTime, inDeltaTime, nullptr, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inAllocator);
-		float test_horizontal_movement_sq = (test_position - up_position).LengthSq();
+		float test_horizontal_movement_sq = Vec3(test_position - up_position).LengthSq();
 		if (test_horizontal_movement_sq <= horizontal_movement_sq + 1.0e-8f)
 			return false; // We didn't move any further than in the previous test
 
@@ -1122,7 +1123,7 @@ bool CharacterVirtual::WalkStairs(float inDeltaTime, Vec3Arg inStepUp, Vec3Arg i
 		// Draw 2nd sweep down
 		if (sDrawWalkStairs)
 		{
-			Vec3 debug_pos = test_position + test_contact.mFraction * down; 
+			RVec3 debug_pos = test_position + test_contact.mFraction * down; 
 			DebugRenderer::sInstance->DrawArrow(test_position, debug_pos, Color::sCyan, 0.01f);
 			DebugRenderer::sInstance->DrawArrow(test_contact.mPosition, test_contact.mPosition + test_contact.mSurfaceNormal, Color::sCyan, 0.01f);
 			mShape->Draw(DebugRenderer::sInstance, GetCenterOfMassTransform(debug_pos, mRotation, mShape), Vec3::sReplicate(1.0f), Color::sCyan, false, true);
@@ -1155,7 +1156,7 @@ bool CharacterVirtual::StickToFloor(Vec3Arg inStepDown, const BroadPhaseLayerFil
 		return false; // If no floor found, don't update our position
 
 	// Calculate new position
-	Vec3 new_position = mPosition + contact.mFraction * inStepDown;
+	RVec3 new_position = mPosition + contact.mFraction * inStepDown;
 
 #ifdef JPH_DEBUG_RENDERER
 	// Draw sweep down
@@ -1178,7 +1179,7 @@ void CharacterVirtual::ExtendedUpdate(float inDeltaTime, Vec3Arg inGravity, cons
 	mLinearVelocity = CancelVelocityTowardsSteepSlopes(desired_velocity);
 	
 	// Remember old position
-	Vec3 old_position = mPosition;
+	RVec3 old_position = mPosition;
 
 	// Track if on ground before the update
 	bool ground_to_air = IsSupported();
@@ -1194,7 +1195,7 @@ void CharacterVirtual::ExtendedUpdate(float inDeltaTime, Vec3Arg inGravity, cons
 	if (ground_to_air && !inSettings.mStickToFloorStepDown.IsNearZero())
 	{
 		// If we're not moving up, stick to the floor
-		float velocity = (mPosition - old_position).Dot(mUp) / inDeltaTime;
+		float velocity = Vec3(mPosition - old_position).Dot(mUp) / inDeltaTime;
 		if (velocity <= 1.0e-6f)
 			StickToFloor(inSettings.mStickToFloorStepDown, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inAllocator);
 	}
@@ -1209,7 +1210,7 @@ void CharacterVirtual::ExtendedUpdate(float inDeltaTime, Vec3Arg inGravity, cons
 		if (desired_horizontal_step_len > 0.0f)
 		{
 			// Calculate how much we moved horizontally
-			Vec3 achieved_horizontal_step = mPosition - old_position;
+			Vec3 achieved_horizontal_step = Vec3(mPosition - old_position);
 			achieved_horizontal_step -= achieved_horizontal_step.Dot(mUp) * mUp;
 
 			// Only count movement in the direction of the desired movement

Jolt/Physics/Character/CharacterVirtual.h

@@ -58,7 +58,7 @@ public:
 	virtual bool						OnContactValidate(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2) { return true; }
 
 	/// Called whenever the character collides with a body. Returns true if the contact can push the character.
-	virtual void						OnContactAdded(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2, Vec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings) { /* Default do nothing */ }
+	virtual void						OnContactAdded(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings) { /* Default do nothing */ }
 
 	/// Called whenever a contact is being used by the solver. Allows the listener to override the resulting character velocity (e.g. by preventing sliding along certain surfaces).
 	/// @param inCharacter Character that is being solved
@@ -70,7 +70,7 @@ public:
 	/// @param inContactMaterial Material of contact point
 	/// @param inCharacterVelocity World space velocity of the character prior to hitting this contact
 	/// @param ioNewCharacterVelocity Contains the calculated world space velocity of the character after hitting this contact, this velocity slides along the surface of the contact. Can be modified by the listener to provide an alternative velocity.
-	virtual void						OnContactSolve(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2, Vec3Arg inContactPosition, Vec3Arg inContactNormal, Vec3Arg inContactVelocity, const PhysicsMaterial *inContactMaterial, Vec3Arg inCharacterVelocity, Vec3 &ioNewCharacterVelocity) { /* Default do nothing */ }
+	virtual void						OnContactSolve(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, Vec3Arg inContactVelocity, const PhysicsMaterial *inContactMaterial, Vec3Arg inCharacterVelocity, Vec3 &ioNewCharacterVelocity) { /* Default do nothing */ }
 };
 
 /// Runtime character object.
@@ -88,7 +88,7 @@ public:
 	/// @param inPosition Initial position for the character
 	/// @param inRotation Initial rotation for the character (usually only around the up-axis)
 	/// @param inSystem Physics system that this character will be added to later
-										CharacterVirtual(const CharacterVirtualSettings *inSettings, Vec3Arg inPosition, QuatArg inRotation, PhysicsSystem *inSystem);
+										CharacterVirtual(const CharacterVirtualSettings *inSettings, RVec3Arg inPosition, QuatArg inRotation, PhysicsSystem *inSystem);
 
 	/// Set the contact listener
 	void								SetListener(CharacterContactListener *inListener)		{ mListener = inListener; }
@@ -103,10 +103,10 @@ public:
 	void								SetLinearVelocity(Vec3Arg inLinearVelocity)				{ mLinearVelocity = inLinearVelocity; }
 
 	/// Get the position of the character
-	Vec3								GetPosition() const										{ return mPosition; }
+	RVec3								GetPosition() const										{ return mPosition; }
 
 	/// Set the position of the character
-	void								SetPosition(Vec3Arg inPosition)							{ mPosition = inPosition; }
+	void								SetPosition(RVec3Arg inPosition)						{ mPosition = inPosition; }
 
 	/// Get the rotation of the character
 	Quat								GetRotation() const										{ return mRotation; }
@@ -115,10 +115,10 @@ public:
 	void								SetRotation(QuatArg inRotation)							{ mRotation = inRotation; }
 
 	/// Calculate the world transform of the character
-	Mat44								GetWorldTransform() const								{ return Mat44::sRotationTranslation(mRotation, mPosition); }
+	RMat44								GetWorldTransform() const								{ return RMat44::sRotationTranslation(mRotation, mPosition); }
 
 	/// Calculates the transform for this character's center of mass
-	Mat44								GetCenterOfMassTransform() const						{ return GetCenterOfMassTransform(mPosition, mRotation, mShape); }
+	RMat44								GetCenterOfMassTransform() const						{ return GetCenterOfMassTransform(mPosition, mRotation, mShape); }
 
 	/// Character mass (kg)
 	void								SetMass(float inMass)									{ mMass = inMass; }
@@ -218,11 +218,12 @@ public:
 	/// @param inMovementDirection A hint in which direction the character is moving, will be used to calculate a proper normal.
 	/// @param inMaxSeparationDistance How much distance around the character you want to report contacts in (can be 0 to match the character exactly).
 	/// @param inShape Shape to test collision with.
+	/// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. GetPosition() since floats are most accurate near the origin
 	/// @param ioCollector Collision collector that receives the collision results.
 	/// @param inBroadPhaseLayerFilter Filter that is used to check if the character collides with something in the broadphase.
 	/// @param inObjectLayerFilter Filter that is used to check if a character collides with a layer.
 	/// @param inBodyFilter Filter that is used to check if a character collides with a body.
-	void								CheckCollision(Vec3Arg inPosition, QuatArg inRotation, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, CollideShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter) const;
+	void								CheckCollision(RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter) const;
 
 	// Saving / restoring state for replay
 	virtual void						SaveState(StateRecorder &inStream) const override;
@@ -238,7 +239,7 @@ private:
 	// Encapsulates a collision contact
 	struct Contact
 	{
-		Vec3							mPosition;												///< Position where the character makes contact
+		RVec3							mPosition;												///< Position where the character makes contact
 		Vec3							mLinearVelocity;										///< Velocity of the contact point
 		Vec3							mContactNormal;											///< Contact normal, pointing towards the character
 		Vec3							mSurfaceNormal;											///< Surface normal of the contact
@@ -285,10 +286,11 @@ private:
 	class ContactCollector : public CollideShapeCollector
 	{
 	public:
-										ContactCollector(PhysicsSystem *inSystem, uint inMaxHits, Vec3Arg inUp, TempContactList &outContacts) : mUp(inUp), mSystem(inSystem), mContacts(outContacts), mMaxHits(inMaxHits) { }
+										ContactCollector(PhysicsSystem *inSystem, uint inMaxHits, Vec3Arg inUp, RVec3Arg inBaseOffset, TempContactList &outContacts) : mBaseOffset(inBaseOffset), mUp(inUp), mSystem(inSystem), mContacts(outContacts), mMaxHits(inMaxHits) { }
 
 		virtual void					AddHit(const CollideShapeResult &inResult) override;
 
+		RVec3							mBaseOffset;
 		Vec3							mUp;
 		PhysicsSystem *					mSystem;
 		TempContactList &				mContacts;
@@ -299,10 +301,11 @@ private:
 	class ContactCastCollector : public CastShapeCollector
 	{
 	public:
-										ContactCastCollector(PhysicsSystem *inSystem, Vec3Arg inDisplacement, uint inMaxHits, Vec3Arg inUp, const IgnoredContactList &inIgnoredContacts, TempContactList &outContacts) : mDisplacement(inDisplacement), mUp(inUp), mSystem(inSystem), mIgnoredContacts(inIgnoredContacts), mContacts(outContacts), mMaxHits(inMaxHits) { }
+										ContactCastCollector(PhysicsSystem *inSystem, Vec3Arg inDisplacement, uint inMaxHits, Vec3Arg inUp, const IgnoredContactList &inIgnoredContacts, RVec3Arg inBaseOffset, TempContactList &outContacts) : mBaseOffset(inBaseOffset), mDisplacement(inDisplacement), mUp(inUp), mSystem(inSystem), mIgnoredContacts(inIgnoredContacts), mContacts(outContacts), mMaxHits(inMaxHits) { }
 
 		virtual void					AddHit(const ShapeCastResult &inResult) override;
 
+		RVec3							mBaseOffset;
 		Vec3							mDisplacement;
 		Vec3							mUp;
 		PhysicsSystem *					mSystem;
@@ -313,10 +316,10 @@ private:
 
 	// Helper function to convert a Jolt collision result into a contact
 	template <class taCollector>
-	inline static void					sFillContactProperties(Contact &outContact, const Body &inBody, Vec3Arg inUp, const taCollector &inCollector, const CollideShapeResult &inResult);
+	inline static void					sFillContactProperties(Contact &outContact, const Body &inBody, Vec3Arg inUp, RVec3Arg inBaseOffset, const taCollector &inCollector, const CollideShapeResult &inResult);
 
 	// Move the shape from ioPosition and try to displace it by inVelocity * inDeltaTime, this will try to slide the shape along the world geometry
-	void								MoveShape(Vec3 &ioPosition, Vec3Arg inVelocity, float inDeltaTime, ContactList *outActiveContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, TempAllocator &inAllocator
+	void								MoveShape(RVec3 &ioPosition, Vec3Arg inVelocity, float inDeltaTime, ContactList *outActiveContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, TempAllocator &inAllocator
 	#ifdef JPH_DEBUG_RENDERER
 		, bool inDrawConstraints = false
 	#endif // JPH_DEBUG_RENDERER
@@ -326,7 +329,7 @@ private:
 	bool								ValidateContact(const Contact &inContact) const;
 
 	// Tests the shape for collision around inPosition
-	void								GetContactsAtPosition(Vec3Arg inPosition, Vec3Arg inMovementDirection, const Shape *inShape, TempContactList &outContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter) const;
+	void								GetContactsAtPosition(RVec3Arg inPosition, Vec3Arg inMovementDirection, const Shape *inShape, TempContactList &outContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter) const;
 
 	// Remove penetrating contacts with the same body that have conflicting normals, leaving these will make the character mover get stuck
 	void								RemoveConflictingContacts(TempContactList &ioContacts, IgnoredContactList &outIgnoredContacts) const;
@@ -345,7 +348,7 @@ private:
 	bool								HandleContact(Vec3Arg inVelocity, Constraint &ioConstraint, float inDeltaTime) const;
 
 	// Does a swept test of the shape from inPosition with displacement inDisplacement, returns true if there was a collision
-	bool								GetFirstContactForSweep(Vec3Arg inPosition, Vec3Arg inDisplacement, Contact &outContact, const IgnoredContactList &inIgnoredContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, TempAllocator &inAllocator) const;
+	bool								GetFirstContactForSweep(RVec3Arg inPosition, Vec3Arg inDisplacement, Contact &outContact, const IgnoredContactList &inIgnoredContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, TempAllocator &inAllocator) const;
 
 	// Store contacts so that we have proper ground information
 	void								StoreActiveContacts(const TempContactList &inContacts, TempAllocator &inAllocator);
@@ -354,12 +357,12 @@ private:
 	void								UpdateSupportingContact(bool inSkipContactVelocityCheck, TempAllocator &inAllocator);
 
 	/// This function can be called after moving the character to a new colliding position
-	void								MoveToContact(Vec3Arg inPosition, const Contact &inContact, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, TempAllocator &inAllocator);
+	void								MoveToContact(RVec3Arg inPosition, const Contact &inContact, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, TempAllocator &inAllocator);
 
 	// This function returns the actual center of mass of the shape, not corrected for the character padding
-	inline Mat44						GetCenterOfMassTransform(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape) const
+	inline RMat44						GetCenterOfMassTransform(RVec3Arg inPosition, QuatArg inRotation, const Shape *inShape) const
 	{
-		return Mat44::sRotationTranslation(inRotation, inPosition).PreTranslated(inShape->GetCenterOfMass()).PostTranslated(mCharacterPadding * mUp);
+		return RMat44::sRotationTranslation(inRotation, inPosition).PreTranslated(inShape->GetCenterOfMass()).PostTranslated(mCharacterPadding * mUp);
 	}
 
 	// Our main listener for contacts
@@ -382,7 +385,7 @@ private:
 	float								mMaxStrength;
 
 	// Current position (of the base, not the center of mass)
-	Vec3								mPosition = Vec3::sZero();
+	RVec3								mPosition = RVec3::sZero();
 
 	// Current rotation (of the base, not of the center of mass)
 	Quat								mRotation = Quat::sIdentity();

Jolt/Physics/Collision/CollideShape.h

@@ -55,15 +55,15 @@ public:
 
 	using Face = StaticArray<Vec3, 32>;
 
-	Vec3						mContactPointOn1;			///< Contact point on the surface of shape 1 (in world space)
-	Vec3						mContactPointOn2;			///< Contact point on the surface of shape 2 (in world space). If the penetration depth is 0, this will be the same as mContactPointOn1.
+	Vec3						mContactPointOn1;			///< Contact point on the surface of shape 1 (in world space or relative to base offset)
+	Vec3						mContactPointOn2;			///< Contact point on the surface of shape 2 (in world space or relative to base offset). If the penetration depth is 0, this will be the same as mContactPointOn1.
 	Vec3						mPenetrationAxis;			///< Direction to move shape 2 out of collision along the shortest path (magnitude is meaningless, in world space). You can use -mPenetrationAxis.Normalized() as contact normal.
 	float						mPenetrationDepth;			///< Penetration depth (move shape 2 by this distance to resolve the collision)
 	SubShapeID					mSubShapeID1;				///< Sub shape ID that identifies the face on shape 1
 	SubShapeID					mSubShapeID2;				///< Sub shape ID that identifies the face on shape 2
 	BodyID						mBodyID2;					///< BodyID to which shape 2 belongs to
-	Face						mShape1Face;				///< Colliding face on shape 1 (optional result, in world space)
-	Face						mShape2Face;				///< Colliding face on shape 2 (optional result, in world space)
+	Face						mShape1Face;				///< Colliding face on shape 1 (optional result, in world space or relative to base offset)
+	Face						mShape2Face;				///< Colliding face on shape 2 (optional result, in world space or relative to base offset)
 };
 
 /// Settings to be passed with a collision query

Jolt/Physics/Collision/ContactListener.h

@@ -19,14 +19,19 @@ class ContactManifold
 {
 public:
 	/// Swaps shape 1 and 2
-	ContactManifold			SwapShapes() const					{ return { -mWorldSpaceNormal, mPenetrationDepth, mSubShapeID2, mSubShapeID1, mWorldSpaceContactPointsOn2, mWorldSpaceContactPointsOn1 }; }
+	ContactManifold			SwapShapes() const					{ return { mBaseOffset, -mWorldSpaceNormal, mPenetrationDepth, mSubShapeID2, mSubShapeID1, mRelativeContactPointsOn2, mRelativeContactPointsOn1 }; }
 
+	/// Access to the world space contact positions
+	inline RVec3			GetWorldSpaceContactPointOn1(uint inIndex) const { return mBaseOffset + mRelativeContactPointsOn1[inIndex]; }
+	inline RVec3			GetWorldSpaceContactPointOn2(uint inIndex) const { return mBaseOffset + mRelativeContactPointsOn2[inIndex]; }
+
+	RVec3					mBaseOffset;						///< Offset to which all the contact points are relative
 	Vec3					mWorldSpaceNormal;					///< Normal for this manifold, direction along which to move body 2 out of collision along the shortest path
 	float					mPenetrationDepth;					///< Penetration depth (move shape 2 by this distance to resolve the collision)
 	SubShapeID				mSubShapeID1;						///< Sub shapes that formed this manifold (note that when multiple manifolds are combined because they're coplanar, we lose some information here because we only keep track of one sub shape pair that we encounter)
 	SubShapeID				mSubShapeID2;
-	ContactPoints			mWorldSpaceContactPointsOn1;		///< Contact points on the surface of shape 1 in world space.
-	ContactPoints			mWorldSpaceContactPointsOn2;		///< Contact points on the surface of shape 2 in world space. If there's no penetration, this will be the same as mWorldSpaceContactPointsOn1. If there is penetration they will be different.
+	ContactPoints			mRelativeContactPointsOn1;			///< Contact points on the surface of shape 1 relative to mBaseOffset.
+	ContactPoints			mRelativeContactPointsOn2;			///< Contact points on the surface of shape 2 relative to mBaseOffset. If there's no penetration, this will be the same as mRelativeContactPointsOn1. If there is penetration they will be different.
 };
 
 /// When a contact point is added or persisted, the callback gets a chance to override certain properties of the contact constraint.

Jolt/Physics/Collision/ManifoldBetweenTwoFaces.cpp

@@ -12,7 +12,7 @@
 
 JPH_NAMESPACE_BEGIN
 
-void PruneContactPoints(Vec3Arg inCenterOfMass, Vec3Arg inPenetrationAxis, ContactPoints &ioContactPointsOn1, ContactPoints &ioContactPointsOn2)
+void PruneContactPoints(Vec3Arg inPenetrationAxis, ContactPoints &ioContactPointsOn1, ContactPoints &ioContactPointsOn2 JPH_IF_DEBUG_RENDERER(, RVec3Arg inCenterOfMass))
 {
 	// Makes no sense to call this with 4 or less points
 	JPH_ASSERT(ioContactPointsOn1.size() > 4);
@@ -32,9 +32,9 @@ void PruneContactPoints(Vec3Arg inCenterOfMass, Vec3Arg inPenetrationAxis, Conta
 	for (ContactPoints::size_type i = 0; i < ioContactPointsOn1.size(); ++i)
 	{
 		// Project contact points on the plane through inCenterOfMass with normal inPenetrationAxis and center around the center of mass of body 1
+		// (note that since all points are relative to inCenterOfMass we can project onto the plane through the origin)
 		Vec3 v1 = ioContactPointsOn1[i]; 
-		Vec3 v1_local = v1 - inCenterOfMass;
-		projected.push_back(v1_local - v1_local.Dot(inPenetrationAxis) * inPenetrationAxis);
+		projected.push_back(v1 - v1.Dot(inPenetrationAxis) * inPenetrationAxis);
 
 		// Calculate penetration depth^2 of each point and clamp against the minimal distance
 		Vec3 v2 = ioContactPointsOn2[i];
@@ -117,14 +117,14 @@ void PruneContactPoints(Vec3Arg inCenterOfMass, Vec3Arg inPenetrationAxis, Conta
 	if (ContactConstraintManager::sDrawContactPointReduction)
 	{
 		// Draw input polygon
-		DebugRenderer::sInstance->DrawWirePolygon(ioContactPointsOn1, Color::sOrange, 0.05f);
+		DebugRenderer::sInstance->DrawWirePolygon(RMat44::sTranslation(inCenterOfMass), ioContactPointsOn1, Color::sOrange, 0.05f);
 
 		// Draw primary axis
-		DebugRenderer::sInstance->DrawArrow(ioContactPointsOn1[point1], ioContactPointsOn1[point2], Color::sRed, 0.05f);
+		DebugRenderer::sInstance->DrawArrow(inCenterOfMass + ioContactPointsOn1[point1], inCenterOfMass + ioContactPointsOn1[point2], Color::sRed, 0.05f);
 
 		// Draw contact points we kept
 		for (Vec3 p : points_to_keep_on_1)
-			DebugRenderer::sInstance->DrawMarker(p, Color::sGreen, 0.1f);
+			DebugRenderer::sInstance->DrawMarker(inCenterOfMass + p, Color::sGreen, 0.1f);
 	}
 #endif // JPH_DEBUG_RENDERER
 
@@ -133,17 +133,20 @@ void PruneContactPoints(Vec3Arg inCenterOfMass, Vec3Arg inPenetrationAxis, Conta
 	ioContactPointsOn2 = points_to_keep_on_2;
 }	
 
-void ManifoldBetweenTwoFaces(Vec3Arg inContactPoint1, Vec3Arg inContactPoint2, Vec3Arg inPenetrationAxis, float inSpeculativeContactDistanceSq, const ConvexShape::SupportingFace &inShape1Face, const ConvexShape::SupportingFace &inShape2Face, ContactPoints &outContactPoints1, ContactPoints &outContactPoints2)
+void ManifoldBetweenTwoFaces(Vec3Arg inContactPoint1, Vec3Arg inContactPoint2, Vec3Arg inPenetrationAxis, float inSpeculativeContactDistanceSq, const ConvexShape::SupportingFace &inShape1Face, const ConvexShape::SupportingFace &inShape2Face, ContactPoints &outContactPoints1, ContactPoints &outContactPoints2 JPH_IF_DEBUG_RENDERER(, RVec3Arg inCenterOfMass))
 {
 #ifdef JPH_DEBUG_RENDERER
 	if (ContactConstraintManager::sDrawContactPoint)
 	{
+		RVec3 cp1 = inCenterOfMass + inContactPoint1;
+		RVec3 cp2 = inCenterOfMass + inContactPoint2;
+
 		// Draw contact points
-		DebugRenderer::sInstance->DrawMarker(inContactPoint1, Color::sRed, 0.1f);
-		DebugRenderer::sInstance->DrawMarker(inContactPoint2, Color::sGreen, 0.1f);
+		DebugRenderer::sInstance->DrawMarker(cp1, Color::sRed, 0.1f);
+		DebugRenderer::sInstance->DrawMarker(cp2, Color::sGreen, 0.1f);
 
 		// Draw contact normal
-		DebugRenderer::sInstance->DrawArrow(inContactPoint1, inContactPoint1 + inPenetrationAxis.Normalized(), Color::sRed, 0.05f);
+		DebugRenderer::sInstance->DrawArrow(cp1, cp1 + inPenetrationAxis.Normalized(), Color::sRed, 0.05f);
 	}
 #endif // JPH_DEBUG_RENDERER
 
@@ -199,20 +202,25 @@ void ManifoldBetweenTwoFaces(Vec3Arg inContactPoint1, Vec3Arg inContactPoint2, V
 	#ifdef JPH_DEBUG_RENDERER
 		if (ContactConstraintManager::sDrawSupportingFaces)
 		{
+			RMat44 com = RMat44::sTranslation(inCenterOfMass);
+
 			// Draw clipped poly
-			DebugRenderer::sInstance->DrawWirePolygon(clipped_face, Color::sOrange);
+			DebugRenderer::sInstance->DrawWirePolygon(com, clipped_face, Color::sOrange);
 
 			// Draw supporting faces
-			DebugRenderer::sInstance->DrawWirePolygon(inShape1Face, Color::sRed, 0.05f);
-			DebugRenderer::sInstance->DrawWirePolygon(inShape2Face, Color::sGreen, 0.05f);
+			DebugRenderer::sInstance->DrawWirePolygon(com, inShape1Face, Color::sRed, 0.05f);
+			DebugRenderer::sInstance->DrawWirePolygon(com, inShape2Face, Color::sGreen, 0.05f);
 
 			// Draw normal
 			if (plane_normal_len_sq > 0.0f)
-				DebugRenderer::sInstance->DrawArrow(plane_origin, plane_origin + plane_normal / sqrt(plane_normal_len_sq), Color::sYellow, 0.05f);
+			{
+				RVec3 plane_origin_ws = inCenterOfMass + plane_origin;
+				DebugRenderer::sInstance->DrawArrow(plane_origin_ws, plane_origin_ws + plane_normal / sqrt(plane_normal_len_sq), Color::sYellow, 0.05f);
+			}
 
 			// Draw contact points that remain after distance check
 			for (ContactPoints::size_type p = old_size; p < outContactPoints1.size(); ++p)
-				DebugRenderer::sInstance->DrawMarker(outContactPoints1[p], Color::sYellow, 0.1f);
+				DebugRenderer::sInstance->DrawMarker(inCenterOfMass + outContactPoints1[p], Color::sYellow, 0.1f);
 		}
 	#endif // JPH_DEBUG_RENDERER
 	}

Jolt/Physics/Collision/ManifoldBetweenTwoFaces.h

@@ -9,22 +9,31 @@
 JPH_NAMESPACE_BEGIN
 
 /// Remove contact points if there are > 4 (no more than 4 are needed for a stable solution)
-/// @param inCenterOfMass is the world space center of mass for body 1
 /// @param inPenetrationAxis is the world space penetration axis (must be normalized)
-/// @param ioContactPointsOn1 The world space contact points on shape 1
-/// @param ioContactPointsOn2 The world space contact points on shape 2
+/// @param ioContactPointsOn1 The contact points on shape 1 relative to inCenterOfMass
+/// @param ioContactPointsOn2 The contact points on shape 2 relative to inCenterOfMass
 /// On output ioContactPointsOn1/2 are reduced to 4 or less points
-void PruneContactPoints(Vec3Arg inCenterOfMass, Vec3Arg inPenetrationAxis, ContactPoints &ioContactPointsOn1, ContactPoints &ioContactPointsOn2);
+/// @param inCenterOfMass Center of mass position of body 1
+void PruneContactPoints(Vec3Arg inPenetrationAxis, ContactPoints &ioContactPointsOn1, ContactPoints &ioContactPointsOn2
+#ifdef JPH_DEBUG_RENDERER
+	, RVec3Arg inCenterOfMass
+#endif
+	);
 
 /// Determine contact points between 2 faces of 2 shapes and return them in outContactPoints 1 & 2
-/// @param inContactPoint1 The contact point on shape 1 in world space
-/// @param inContactPoint2 The contact point on shape 2 in world space
+/// @param inContactPoint1 The contact point on shape 1 relative to inCenterOfMass
+/// @param inContactPoint2 The contact point on shape 2 relative to inCenterOfMass
 /// @param inPenetrationAxis The local space penetration axis in world space
 /// @param inSpeculativeContactDistanceSq Squared speculative contact distance, any contact further apart than this distance will be discarded
-/// @param inShape1Face The supporting faces on shape 1 in world space
-/// @param inShape2Face The supporting faces on shape 2 in world space
-/// @param outContactPoints1 Returns the contact points between the two shapes for shape 1 in world space (any existing points in the output array are left as is)
-/// @param outContactPoints2 Returns the contact points between the two shapes for shape 2 in world space (any existing points in the output array are left as is)
-void ManifoldBetweenTwoFaces(Vec3Arg inContactPoint1, Vec3Arg inContactPoint2, Vec3Arg inPenetrationAxis, float inSpeculativeContactDistanceSq, const ConvexShape::SupportingFace &inShape1Face, const ConvexShape::SupportingFace &inShape2Face, ContactPoints &outContactPoints1, ContactPoints &outContactPoints2);
+/// @param inShape1Face The supporting faces on shape 1 relative to inCenterOfMass
+/// @param inShape2Face The supporting faces on shape 2 relative to inCenterOfMass
+/// @param outContactPoints1 Returns the contact points between the two shapes for shape 1 relative to inCenterOfMass (any existing points in the output array are left as is)
+/// @param outContactPoints2 Returns the contact points between the two shapes for shape 2 relative to inCenterOfMass (any existing points in the output array are left as is)
+/// @param inCenterOfMass Center of mass position of body 1
+void ManifoldBetweenTwoFaces(Vec3Arg inContactPoint1, Vec3Arg inContactPoint2, Vec3Arg inPenetrationAxis, float inSpeculativeContactDistanceSq, const ConvexShape::SupportingFace &inShape1Face, const ConvexShape::SupportingFace &inShape2Face, ContactPoints &outContactPoints1, ContactPoints &outContactPoints2
+#ifdef JPH_DEBUG_RENDERER
+	, RVec3Arg inCenterOfMass
+#endif
+	);
 
 JPH_NAMESPACE_END

Jolt/Physics/Collision/NarrowPhaseQuery.cpp

@@ -14,14 +14,14 @@
 
 JPH_NAMESPACE_BEGIN
 
-bool NarrowPhaseQuery::CastRay(const RayCast &inRay, RayCastResult &ioHit, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter) const
+bool NarrowPhaseQuery::CastRay(const RRayCast &inRay, RayCastResult &ioHit, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter) const
 {
 	JPH_PROFILE_FUNCTION();
 
 	class MyCollector : public RayCastBodyCollector
 	{
 	public:
-							MyCollector(const RayCast &inRay, RayCastResult &ioHit, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter) :
+							MyCollector(const RRayCast &inRay, RayCastResult &ioHit, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter) :
 			mRay(inRay),
 			mHit(ioHit),
 			mBodyLockInterface(inBodyLockInterface),
@@ -66,26 +66,26 @@ bool NarrowPhaseQuery::CastRay(const RayCast &inRay, RayCastResult &ioHit, const
 			}
 		}
 
-		RayCast						mRay;
+		RRayCast					mRay;
 		RayCastResult &				mHit;
 		const BodyLockInterface &	mBodyLockInterface;
 		const BodyFilter &			mBodyFilter;
 	};
 	
-	// Do broadphase test
+	// Do broadphase test, note that the broadphase uses floats so we drop precision here
 	MyCollector collector(inRay, ioHit, *mBodyLockInterface, inBodyFilter);
-	mBroadPhase->CastRay(inRay, collector, inBroadPhaseLayerFilter, inObjectLayerFilter);
+	mBroadPhase->CastRay(RayCast(inRay), collector, inBroadPhaseLayerFilter, inObjectLayerFilter);
 	return ioHit.mFraction <= 1.0f;
 }
 
-void NarrowPhaseQuery::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, CastRayCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const
+void NarrowPhaseQuery::CastRay(const RRayCast &inRay, const RayCastSettings &inRayCastSettings, CastRayCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const
 {
 	JPH_PROFILE_FUNCTION();
 
 	class MyCollector : public RayCastBodyCollector
 	{
 	public:
-							MyCollector(const RayCast &inRay, const RayCastSettings &inRayCastSettings, CastRayCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) :
+							MyCollector(const RRayCast &inRay, const RayCastSettings &inRayCastSettings, CastRayCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) :
 			mRay(inRay),
 			mRayCastSettings(inRayCastSettings),
 			mCollector(ioCollector),
@@ -131,7 +131,7 @@ void NarrowPhaseQuery::CastRay(const RayCast &inRay, const RayCastSettings &inRa
 			}
 		}
 
-		RayCast						mRay;
+		RRayCast					mRay;
 		RayCastSettings				mRayCastSettings;
 		CastRayCollector &			mCollector;
 		const BodyLockInterface &	mBodyLockInterface;
@@ -139,19 +139,19 @@ void NarrowPhaseQuery::CastRay(const RayCast &inRay, const RayCastSettings &inRa
 		const ShapeFilter &			mShapeFilter;
 	};
 
-	// Do broadphase test
+	// Do broadphase test, note that the broadphase uses floats so we drop precision here
 	MyCollector collector(inRay, inRayCastSettings, ioCollector, *mBodyLockInterface, inBodyFilter, inShapeFilter);
-	mBroadPhase->CastRay(inRay, collector, inBroadPhaseLayerFilter, inObjectLayerFilter);
+	mBroadPhase->CastRay(RayCast(inRay), collector, inBroadPhaseLayerFilter, inObjectLayerFilter);
 }
 
-void NarrowPhaseQuery::CollidePoint(Vec3Arg inPoint, CollidePointCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const
+void NarrowPhaseQuery::CollidePoint(RVec3Arg inPoint, CollidePointCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const
 {
 	JPH_PROFILE_FUNCTION();
 
 	class MyCollector : public CollideShapeBodyCollector
 	{
 	public:
-							MyCollector(Vec3Arg inPoint, CollidePointCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) :
+							MyCollector(RVec3Arg inPoint, CollidePointCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) :
 			mPoint(inPoint),
 			mCollector(ioCollector),
 			mBodyLockInterface(inBodyLockInterface),
@@ -193,30 +193,31 @@ void NarrowPhaseQuery::CollidePoint(Vec3Arg inPoint, CollidePointCollector &ioCo
 			}
 		}
 
-		Vec3							mPoint;
+		RVec3							mPoint;
 		CollidePointCollector &			mCollector;
 		const BodyLockInterface &		mBodyLockInterface;
 		const BodyFilter &				mBodyFilter;
 		const ShapeFilter &				mShapeFilter;
 	};
 
-	// Do broadphase test
+	// Do broadphase test (note: truncates double to single precision since the broadphase uses single precision)
 	MyCollector collector(inPoint, ioCollector, *mBodyLockInterface, inBodyFilter, inShapeFilter);
-	mBroadPhase->CollidePoint(inPoint, collector, inBroadPhaseLayerFilter, inObjectLayerFilter);
+	mBroadPhase->CollidePoint(Vec3(inPoint), collector, inBroadPhaseLayerFilter, inObjectLayerFilter);
 }
 
-void NarrowPhaseQuery::CollideShape(const Shape *inShape, Vec3Arg inShapeScale, Mat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const
+void NarrowPhaseQuery::CollideShape(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const
 {
 	JPH_PROFILE_FUNCTION();
 
 	class MyCollector : public CollideShapeBodyCollector
 	{
 	public:
-							MyCollector(const Shape *inShape, Vec3Arg inShapeScale, Mat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) :
+							MyCollector(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) :
 			mShape(inShape),
 			mShapeScale(inShapeScale),
 			mCenterOfMassTransform(inCenterOfMassTransform),
 			mCollideShapeSettings(inCollideShapeSettings),
+			mBaseOffset(inBaseOffset),
 			mCollector(ioCollector),
 			mBodyLockInterface(inBodyLockInterface),
 			mBodyFilter(inBodyFilter),
@@ -248,7 +249,7 @@ void NarrowPhaseQuery::CollideShape(const Shape *inShape, Vec3Arg inShapeScale,
 						lock.ReleaseLock();
 
 						// Do narrow phase collision check
-						ts.CollideShape(mShape, mShapeScale, mCenterOfMassTransform, mCollideShapeSettings, mCollector, mShapeFilter);
+						ts.CollideShape(mShape, mShapeScale, mCenterOfMassTransform, mCollideShapeSettings, mBaseOffset, mCollector, mShapeFilter);
 
 						// Update early out fraction based on narrow phase collector
 						UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction());
@@ -259,8 +260,9 @@ void NarrowPhaseQuery::CollideShape(const Shape *inShape, Vec3Arg inShapeScale,
 
 		const Shape *					mShape;
 		Vec3							mShapeScale;
-		Mat44							mCenterOfMassTransform;
+		RMat44							mCenterOfMassTransform;
 		const CollideShapeSettings &	mCollideShapeSettings;
+		RVec3							mBaseOffset;
 		CollideShapeCollector &			mCollector;
 		const BodyLockInterface &		mBodyLockInterface;
 		const BodyFilter &				mBodyFilter;
@@ -272,11 +274,11 @@ void NarrowPhaseQuery::CollideShape(const Shape *inShape, Vec3Arg inShapeScale,
 	bounds.ExpandBy(Vec3::sReplicate(inCollideShapeSettings.mMaxSeparationDistance));
 
 	// Do broadphase test
-	MyCollector collector(inShape, inShapeScale, inCenterOfMassTransform, inCollideShapeSettings, ioCollector, *mBodyLockInterface, inBodyFilter, inShapeFilter);
+	MyCollector collector(inShape, inShapeScale, inCenterOfMassTransform, inCollideShapeSettings, inBaseOffset, ioCollector, *mBodyLockInterface, inBodyFilter, inShapeFilter);
 	mBroadPhase->CollideAABox(bounds, collector, inBroadPhaseLayerFilter, inObjectLayerFilter);
 }
 
-void NarrowPhaseQuery::CastShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, CastShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const
+void NarrowPhaseQuery::CastShape(const RShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const
 {
 	JPH_PROFILE_FUNCTION();
 
@@ -294,9 +296,10 @@ void NarrowPhaseQuery::CastShape(const ShapeCast &inShapeCast, const ShapeCastSe
 			}
 
 	public:
-							MyCollector(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, CastShapeCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) :
+							MyCollector(const RShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) :
 			mShapeCast(inShapeCast),
 			mShapeCastSettings(inShapeCastSettings),
+			mBaseOffset(inBaseOffset),
 			mCollector(ioCollector),
 			mBodyLockInterface(inBodyLockInterface),
 			mBodyFilter(inBodyFilter),
@@ -331,7 +334,7 @@ void NarrowPhaseQuery::CastShape(const ShapeCast &inShapeCast, const ShapeCastSe
 						lock.ReleaseLock();
 
 						// Do narrow phase collision check
-						ts.CastShape(mShapeCast, mShapeCastSettings, mCollector, mShapeFilter);
+						ts.CastShape(mShapeCast, mShapeCastSettings, mBaseOffset, mCollector, mShapeFilter);
 
 						// Update early out fraction based on narrow phase collector
 						PropagateEarlyOutFraction();
@@ -340,8 +343,9 @@ void NarrowPhaseQuery::CastShape(const ShapeCast &inShapeCast, const ShapeCastSe
 			}
 		}
 
-		ShapeCast					mShapeCast;
+		RShapeCast					mShapeCast;
 		const ShapeCastSettings &	mShapeCastSettings;
+		RVec3						mBaseOffset;
 		CastShapeCollector &		mCollector;
 		const BodyLockInterface &	mBodyLockInterface;
 		const BodyFilter &			mBodyFilter;
@@ -349,7 +353,7 @@ void NarrowPhaseQuery::CastShape(const ShapeCast &inShapeCast, const ShapeCastSe
 	};
 
 	// Do broadphase test
-	MyCollector collector(inShapeCast, inShapeCastSettings, ioCollector, *mBodyLockInterface, inBodyFilter, inShapeFilter);
+	MyCollector collector(inShapeCast, inShapeCastSettings, inBaseOffset, ioCollector, *mBodyLockInterface, inBodyFilter, inShapeFilter);
 	mBroadPhase->CastAABox({ inShapeCast.mShapeWorldBounds, inShapeCast.mDirection }, collector, inBroadPhaseLayerFilter, inObjectLayerFilter);
 }
 

Jolt/Physics/Collision/NarrowPhaseQuery.h

@@ -26,22 +26,41 @@ public:
 	/// Cast a ray and find the closest hit. Returns true if it finds a hit. Hits further than ioHit.mFraction will not be considered and in this case ioHit will remain unmodified (and the function will return false).
 	/// Convex objects will be treated as solid (meaning if the ray starts inside, you'll get a hit fraction of 0) and back face hits against triangles are returned.
 	/// If you want the surface normal of the hit use Body::GetWorldSpaceSurfaceNormal(ioHit.mSubShapeID2, inRay.GetPointOnRay(ioHit.mFraction)) on body with ID ioHit.mBodyID.
-	bool						CastRay(const RayCast &inRay, RayCastResult &ioHit, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }) const;
+	bool						CastRay(const RRayCast &inRay, RayCastResult &ioHit, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }) const;
 
 	/// Cast a ray, allows collecting multiple hits. Note that this version is more flexible but also slightly slower than the CastRay function that returns only a single hit.
 	/// If you want the surface normal of the hit use Body::GetWorldSpaceSurfaceNormal(collected sub shape ID, inRay.GetPointOnRay(collected fraction)) on body with collected body ID.
-	void						CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, CastRayCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const;
+	void						CastRay(const RRayCast &inRay, const RayCastSettings &inRayCastSettings, CastRayCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const;
 
 	/// Check if inPoint is inside any shapes. For this tests all shapes are treated as if they were solid. 
 	/// For a mesh shape, this test will only provide sensible information if the mesh is a closed manifold.
 	/// For each shape that collides, ioCollector will receive a hit
-	void						CollidePoint(Vec3Arg inPoint, CollidePointCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const;
+	void						CollidePoint(RVec3Arg inPoint, CollidePointCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const;
 
 	/// Collide a shape with the system
-	void						CollideShape(const Shape *inShape, Vec3Arg inShapeScale, Mat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const;
+	/// @param inShape Shape to test
+	/// @param inShapeScale Scale in local space of shape
+	/// @param inCenterOfMassTransform Center of mass transform for the shape
+	/// @param inCollideShapeSettings Settings
+	/// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. inCenterOfMassTransform.GetTranslation() since floats are most accurate near the origin
+	/// @param ioCollector Collector that receives the hits
+	/// @param inBroadPhaseLayerFilter Filter that filters at broadphase level
+	/// @param inObjectLayerFilter Filter that filters at layer level
+	/// @param inBodyFilter Filter that filters at body level
+	/// @param inShapeFilter Filter that filters at shape level
+	void						CollideShape(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const;
+
 
 	/// Cast a shape and report any hits to ioCollector
-	void						CastShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, CastShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const;
+	/// @param inShapeCast The shape cast and its position and direction
+	/// @param inShapeCastSettings Settings for the shape cast
+	/// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. inShapeCast.mCenterOfMassStart.GetTranslation() since floats are most accurate near the origin
+	/// @param ioCollector Collector that receives the hits
+	/// @param inBroadPhaseLayerFilter Filter that filters at broadphase level
+	/// @param inObjectLayerFilter Filter that filters at layer level
+	/// @param inBodyFilter Filter that filters at body level
+	/// @param inShapeFilter Filter that filters at shape level
+	void						CastShape(const RShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const;
 
 	/// Collect all leaf transformed shapes that fall inside world space box inBox
 	void						CollectTransformedShapes(const AABox &inBox, TransformedShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const;

Jolt/Physics/Collision/RayCast.h

@@ -8,28 +8,55 @@
 JPH_NAMESPACE_BEGIN
 
 /// Structure that holds a single ray cast
-struct RayCast
+template <class Vec, class Mat, class RayCastType>
+struct RayCastT
 {
 	JPH_OVERRIDE_NEW_DELETE
 
+	/// Constructors
+								RayCastT(typename Vec::ArgType inOrigin, Vec3Arg inDirection) : mOrigin(inOrigin), mDirection(inDirection) { }
+								RayCastT(const RayCastT<Vec, Mat, RayCastType> &) = default;
+
 	/// Transform this ray using inTransform
-	RayCast						Transformed(Mat44Arg inTransform) const
+	RayCastType					Transformed(typename Mat::ArgType inTransform) const
 	{
-		Vec3 ray_origin = inTransform * mOrigin;
-		Vec3 ray_direction = inTransform * (mOrigin + mDirection) - ray_origin;
+		Vec ray_origin = inTransform * mOrigin;
+		Vec3 ray_direction(inTransform * (mOrigin + mDirection) - ray_origin);
 		return { ray_origin, ray_direction };
 	}
 
 	/// Get point with fraction inFraction on ray (0 = start of ray, 1 = end of ray)
-	inline Vec3					GetPointOnRay(float inFraction) const
+	inline Vec					GetPointOnRay(float inFraction) const
 	{
 		return mOrigin + inFraction * mDirection;
 	}
 
-	Vec3						mOrigin;					///< Origin of the ray
+	Vec							mOrigin;					///< Origin of the ray
 	Vec3						mDirection;					///< Direction and length of the ray (anything beyond this length will not be reported as a hit)
 };
 
+struct RayCast : public RayCastT<Vec3, Mat44, RayCast>
+{
+	using RayCastT<Vec3, Mat44, RayCast>::RayCastT;
+};
+
+struct RRayCast : public RayCastT<RVec3, RMat44, RRayCast>
+{
+	using RayCastT<RVec3, RMat44, RRayCast>::RayCastT;
+
+	/// Convert from RayCast, converts single to double precision
+	explicit					RRayCast(const RayCast &inRay) :
+		RRayCast(RVec3(inRay.mOrigin), inRay.mDirection)
+	{
+	}
+
+	/// Convert to RayCast, which implies casting from double precision to single precision
+	explicit					operator RayCast() const
+	{
+		return RayCast(Vec3(mOrigin), mDirection);
+	}
+};
+
 /// Settings to be passed with a ray cast
 class RayCastSettings
 {

Jolt/Physics/Collision/Shape/BoxShape.cpp

@@ -158,7 +158,7 @@ Vec3 BoxShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalS
 }
 
 #ifdef JPH_DEBUG_RENDERER
-void BoxShape::Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+void BoxShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
 {
 	DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid;
 	inRenderer->DrawBox(inCenterOfMassTransform * Mat44::sScale(inScale.Abs()), GetLocalBounds(), inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor, DebugRenderer::ECastShadow::On, draw_mode);

Jolt/Physics/Collision/Shape/BoxShape.h

@@ -65,7 +65,7 @@ public:
 
 #ifdef JPH_DEBUG_RENDERER
 	// See Shape::Draw
-	virtual void			Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+	virtual void			Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
 #endif // JPH_DEBUG_RENDERER
 
 	// See Shape::CastRay

Jolt/Physics/Collision/Shape/CapsuleShape.cpp

@@ -276,7 +276,7 @@ AABox CapsuleShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Ar
 }
 
 #ifdef JPH_DEBUG_RENDERER
-void CapsuleShape::Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+void CapsuleShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
 {
 	DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid;
 	inRenderer->DrawCapsule(inCenterOfMassTransform * Mat44::sScale(inScale.Abs().GetX()), mHalfHeightOfCylinder, mRadius, inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor, DebugRenderer::ECastShadow::On, draw_mode);
@@ -324,7 +324,7 @@ void CapsuleShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedS
 {
 	Vec3 scale;
 	Mat44 transform = inCenterOfMassTransform.Decompose(scale);
-	TransformedShape ts(transform.GetTranslation(), transform.GetRotation().GetQuaternion(), this, BodyID(), SubShapeIDCreator());
+	TransformedShape ts(RVec3(transform.GetTranslation()), transform.GetRotation().GetQuaternion(), this, BodyID(), SubShapeIDCreator());
 	ts.SetShapeScale(ScaleHelpers::MakeUniformScale(scale.Abs()));
 	ioCollector.AddHit(ts);
 }

Jolt/Physics/Collision/Shape/CapsuleShape.h

@@ -55,6 +55,7 @@ public:
 		
 	// See Shape::GetWorldSpaceBounds
 	virtual AABox			GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
+	using Shape::GetWorldSpaceBounds;
 
 	// See Shape::GetInnerRadius
 	virtual float			GetInnerRadius() const override											{ return mRadius; }
@@ -73,7 +74,7 @@ public:
 
 #ifdef JPH_DEBUG_RENDERER
 	// See Shape::Draw
-	virtual void			Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+	virtual void			Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
 #endif // JPH_DEBUG_RENDERER
 
 	// See Shape::CastRay

Jolt/Physics/Collision/Shape/CompoundShape.cpp

@@ -150,7 +150,7 @@ TransformedShape CompoundShape::GetSubShapeTransformedShape(const SubShapeID &in
 	Vec3 scale = sub_shape.TransformScale(inScale);
 
 	// Return transformed shape
-	TransformedShape ts(position, rotation, sub_shape.mShape, BodyID());
+	TransformedShape ts(RVec3(position), rotation, sub_shape.mShape, BodyID());
 	ts.SetShapeScale(scale);
 	return ts;
 }
@@ -182,7 +182,7 @@ void CompoundShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg in
 	shape.mShape->GetSupportingFace(remainder, transform.Multiply3x3Transposed(inDirection), shape.TransformScale(inScale), inCenterOfMassTransform * transform, outVertices);
 }
 
-void CompoundShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy) const
+void CompoundShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const
 {
 	outTotalVolume = 0.0f;
 	outSubmergedVolume = 0.0f;
@@ -196,7 +196,7 @@ void CompoundShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg
 		// Recurse to child
 		float total_volume, submerged_volume;
 		Vec3 center_of_buoyancy;
-		shape.mShape->GetSubmergedVolume(transform, shape.TransformScale(inScale), inSurface, total_volume, submerged_volume, center_of_buoyancy);
+		shape.mShape->GetSubmergedVolume(transform, shape.TransformScale(inScale), inSurface, total_volume, submerged_volume, center_of_buoyancy JPH_IF_DEBUG_RENDERER(, inBaseOffset));
 
 		// Accumulate volumes
 		outTotalVolume += total_volume;
@@ -212,12 +212,12 @@ void CompoundShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg
 #ifdef JPH_DEBUG_RENDERER
 	// Draw senter of buoyancy
 	if (sDrawSubmergedVolumes)
-		DebugRenderer::sInstance->DrawWireSphere(outCenterOfBuoyancy, 0.05f, Color::sRed, 1);
+		DebugRenderer::sInstance->DrawWireSphere(inBaseOffset + outCenterOfBuoyancy, 0.05f, Color::sRed, 1);
 #endif // JPH_DEBUG_RENDERER
 }
 
 #ifdef JPH_DEBUG_RENDERER
-void CompoundShape::Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+void CompoundShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
 {
 	for (const SubShape &shape : mSubShapes)
 	{
@@ -226,7 +226,7 @@ void CompoundShape::Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTrans
 	}
 }
 
-void CompoundShape::DrawGetSupportFunction(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const
+void CompoundShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const
 {
 	for (const SubShape &shape : mSubShapes)
 	{
@@ -235,7 +235,7 @@ void CompoundShape::DrawGetSupportFunction(DebugRenderer *inRenderer, Mat44Arg i
 	}
 }
 
-void CompoundShape::DrawGetSupportingFace(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
+void CompoundShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
 {
 	for (const SubShape &shape : mSubShapes)
 	{

Jolt/Physics/Collision/Shape/CompoundShape.h

@@ -67,6 +67,7 @@ public:
 
 	// See Shape::GetWorldSpaceBounds
 	virtual AABox					GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
+	using Shape::GetWorldSpaceBounds;
 
 	// See Shape::GetInnerRadius
 	virtual float					GetInnerRadius() const override							{ return mInnerRadius; }
@@ -90,17 +91,17 @@ public:
 	virtual void					GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override;
 
 	// See Shape::GetSubmergedVolume
-	virtual void					GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy) const override;
+	virtual void					GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override;
 
 #ifdef JPH_DEBUG_RENDERER
 	// See Shape::Draw
-	virtual void					Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+	virtual void					Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
 
 	// See Shape::DrawGetSupportFunction
-	virtual void					DrawGetSupportFunction(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const override;
+	virtual void					DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const override;
 
 	// See Shape::DrawGetSupportingFace
-	virtual void					DrawGetSupportingFace(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
+	virtual void					DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
 #endif // JPH_DEBUG_RENDERER
 
 	// See Shape::TransformShape

Jolt/Physics/Collision/Shape/ConvexHullShape.cpp

@@ -720,7 +720,7 @@ void ConvexHullShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg
 	}
 }
 
-void ConvexHullShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy) const
+void ConvexHullShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const
 {
 	// Trivially calculate total volume
 	Vec3 abs_scale = inScale.Abs();
@@ -732,7 +732,7 @@ void ConvexHullShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3A
 	// Convert the points to world space and determine the distance to the surface
 	int num_points = int(mPoints.size());
 	PolyhedronSubmergedVolumeCalculator::Point *buffer = (PolyhedronSubmergedVolumeCalculator::Point *)JPH_STACK_ALLOC(num_points * sizeof(PolyhedronSubmergedVolumeCalculator::Point));
-	PolyhedronSubmergedVolumeCalculator submerged_vol_calc(inCenterOfMassTransform * Mat44::sScale(inScale), &mPoints[0].mPosition, sizeof(Point), num_points, inSurface, buffer);
+	PolyhedronSubmergedVolumeCalculator submerged_vol_calc(inCenterOfMassTransform * Mat44::sScale(inScale), &mPoints[0].mPosition, sizeof(Point), num_points, inSurface, buffer JPH_IF_DEBUG_RENDERER(, inBaseOffset));
 	
 	if (submerged_vol_calc.AreAllAbove())
 	{
@@ -797,12 +797,12 @@ void ConvexHullShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3A
 #ifdef JPH_DEBUG_RENDERER
 	// Draw center of buoyancy
 	if (sDrawSubmergedVolumes)
-		DebugRenderer::sInstance->DrawWireSphere(outCenterOfBuoyancy, 0.05f, Color::sRed, 1);
+		DebugRenderer::sInstance->DrawWireSphere(inBaseOffset + outCenterOfBuoyancy, 0.05f, Color::sRed, 1);
 #endif // JPH_DEBUG_RENDERER
 }
 
 #ifdef JPH_DEBUG_RENDERER
-void ConvexHullShape::Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+void ConvexHullShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
 {
 	if (mGeometry == nullptr)
 	{
@@ -834,7 +834,7 @@ void ConvexHullShape::Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTra
 
 	// Draw the geometry
 	Color color = inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor;
-	Mat44 transform = inCenterOfMassTransform * Mat44::sScale(inScale);
+	RMat44 transform = inCenterOfMassTransform.PreScaled(inScale);
 	inRenderer->DrawGeometry(transform, color, mGeometry, cull_mode, DebugRenderer::ECastShadow::On, draw_mode);
 
 	// Draw the outline if requested
@@ -851,19 +851,19 @@ void ConvexHullShape::Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTra
 		}
 }
 
-void ConvexHullShape::DrawShrunkShape(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
+void ConvexHullShape::DrawShrunkShape(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
 {
 	// Get the shrunk points
 	SupportBuffer buffer;
 	const HullNoConvex *support = mConvexRadius > 0.0f? static_cast<const HullNoConvex *>(GetSupportFunction(ESupportMode::ExcludeConvexRadius, buffer, inScale)) : nullptr;
 
-	Mat44 transform = inCenterOfMassTransform * Mat44::sScale(inScale);
+	RMat44 transform = inCenterOfMassTransform * Mat44::sScale(inScale);
 
 	for (int p = 0; p < (int)mPoints.size(); ++p)
 	{
 		const Point &point = mPoints[p];
-		Vec3 position = transform * point.mPosition;
-		Vec3 shrunk_point = support != nullptr? transform * support->GetPoints()[p] : position;
+		RVec3 position = transform * point.mPosition;
+		RVec3 shrunk_point = support != nullptr? transform * support->GetPoints()[p] : position;
 
 		// Draw difference between shrunk position and position
 		inRenderer->DrawLine(position, shrunk_point, Color::sGreen);

Jolt/Physics/Collision/Shape/ConvexHullShape.h

@@ -71,14 +71,14 @@ public:
 	virtual const Support *	GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override;
 
 	// See Shape::GetSubmergedVolume
-	virtual void			GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy) const override;
+	virtual void			GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override;
 
 #ifdef JPH_DEBUG_RENDERER
 	// See Shape::Draw
-	virtual void			Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+	virtual void			Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
 
 	/// Debugging helper draw function that draws how all points are moved when a shape is shrunk by the convex radius
-	void					DrawShrunkShape(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const;
+	void					DrawShrunkShape(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const;
 #endif // JPH_DEBUG_RENDERER
 
 	// See Shape::CastRay

Jolt/Physics/Collision/Shape/ConvexShape.cpp

@@ -368,7 +368,7 @@ int ConvexShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrian
 	return total_num_triangles;
 }
 
-void ConvexShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy) const
+void ConvexShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const
 {
 	// Calculate total volume
 	Vec3 abs_scale = inScale.Abs();
@@ -402,7 +402,7 @@ void ConvexShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg i
 	};
 
 	PolyhedronSubmergedVolumeCalculator::Point *buffer = (PolyhedronSubmergedVolumeCalculator::Point *)JPH_STACK_ALLOC(8 * sizeof(PolyhedronSubmergedVolumeCalculator::Point));
-	PolyhedronSubmergedVolumeCalculator submerged_vol_calc(inCenterOfMassTransform * Mat44::sScale(extent), points, sizeof(Vec3), 8, inSurface, buffer);
+	PolyhedronSubmergedVolumeCalculator submerged_vol_calc(inCenterOfMassTransform * Mat44::sScale(extent), points, sizeof(Vec3), 8, inSurface, buffer JPH_IF_DEBUG_RENDERER(, inBaseOffset));
 
 	if (submerged_vol_calc.AreAllAbove())
 	{
@@ -436,7 +436,7 @@ void ConvexShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg i
 }
 
 #ifdef JPH_DEBUG_RENDERER
-void ConvexShape::DrawGetSupportFunction(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const
+void ConvexShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const
 {
 	// Get the support function with convex radius
 	SupportBuffer buffer;
@@ -456,15 +456,15 @@ void ConvexShape::DrawGetSupportFunction(DebugRenderer *inRenderer, Mat44Arg inC
 		{
 			Vec3 direction = 0.05f * v;
 			Vec3 pos = add_convex.GetSupport(direction);
-			Vec3 from = inCenterOfMassTransform * pos;
-			Vec3 to = inCenterOfMassTransform * (pos + direction);
+			RVec3 from = inCenterOfMassTransform * pos;
+			RVec3 to = inCenterOfMassTransform * (pos + direction);
 			inRenderer->DrawMarker(from, Color::sWhite, 0.001f);
 			inRenderer->DrawArrow(from, to, Color::sWhite, 0.001f);
 		}
 	}
 }
 
-void ConvexShape::DrawGetSupportingFace(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
+void ConvexShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
 {
 	// Sample directions and map which faces belong to which directions
 	using FaceToDirection = UnorderedMap<SupportingFace, Array<Vec3>>;
@@ -497,16 +497,16 @@ void ConvexShape::DrawGetSupportingFace(DebugRenderer *inRenderer, Mat44Arg inCe
 		Vec3 displacement = 0.001f * normal;
 		
 		// Transform face to world space and calculate center of mass
-		Vec3 com = Vec3::sZero();
+		Vec3 com_ls = Vec3::sZero();
 		for (Vec3 &v : face)
 		{
-			v = inCenterOfMassTransform * (v + displacement);
-			com += v;
+			v = inCenterOfMassTransform.Multiply3x3(v + displacement);
+			com_ls += v;
 		}
-		com /= (float)face.size();
+		RVec3 com = inCenterOfMassTransform.GetTranslation() + com_ls / (float)face.size();
 		
 		// Draw the polygon and directions
-		inRenderer->DrawWirePolygon(face, color, face.size() >= 3? 0.001f : 0.0f);
+		inRenderer->DrawWirePolygon(RMat44::sTranslation(inCenterOfMassTransform.GetTranslation()), face, color, face.size() >= 3? 0.001f : 0.0f);
 		if (face.size() >= 3)
 			inRenderer->DrawArrow(com, com + inCenterOfMassTransform.Multiply3x3(normal), color, 0.01f);
 		for (Vec3 &v : ftd.second)

Jolt/Physics/Collision/Shape/ConvexShape.h

@@ -61,7 +61,7 @@ public:
 	virtual int						GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override;
 
 	// See Shape::GetSubmergedVolume
-	virtual void					GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy) const override;
+	virtual void					GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override;
 
 	/// Function that provides an interface for GJK
 	class Support
@@ -111,10 +111,10 @@ public:
 
 #ifdef JPH_DEBUG_RENDERER
 	// See Shape::DrawGetSupportFunction
-	virtual void					DrawGetSupportFunction(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const override;
+	virtual void					DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const override;
 
 	// See Shape::DrawGetSupportingFace
-	virtual void					DrawGetSupportingFace(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
+	virtual void					DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
 #endif // JPH_DEBUG_RENDERER
 
 	// See Shape

Jolt/Physics/Collision/Shape/CylinderShape.cpp

@@ -266,7 +266,7 @@ AABox CylinderShape::GetLocalBounds() const
 }
 
 #ifdef JPH_DEBUG_RENDERER
-void CylinderShape::Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+void CylinderShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
 {
 	DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid;
 	inRenderer->DrawCylinder(inCenterOfMassTransform * Mat44::sScale(inScale.Abs()), mHalfHeight, mRadius, inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor, DebugRenderer::ECastShadow::On, draw_mode);
@@ -302,7 +302,7 @@ void CylinderShape::TransformShape(Mat44Arg inCenterOfMassTransform, Transformed
 {
 	Vec3 scale;
 	Mat44 transform = inCenterOfMassTransform.Decompose(scale);
-	TransformedShape ts(transform.GetTranslation(), transform.GetRotation().GetQuaternion(), this, BodyID(), SubShapeIDCreator());
+	TransformedShape ts(RVec3(transform.GetTranslation()), transform.GetRotation().GetQuaternion(), this, BodyID(), SubShapeIDCreator());
 	Vec3 abs_scale = scale.Abs();
 	float xz = 0.5f * (abs_scale.GetX() + abs_scale.GetZ());
 	ts.SetShapeScale(Vec3(xz, abs_scale.GetY(), xz));

Jolt/Physics/Collision/Shape/CylinderShape.h

@@ -69,7 +69,7 @@ public:
 
 #ifdef JPH_DEBUG_RENDERER
 	// See Shape::Draw
-	virtual void			Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+	virtual void			Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
 #endif // JPH_DEBUG_RENDERER
 
 	// See Shape::CastRay

Jolt/Physics/Collision/Shape/HeightFieldShape.cpp

@@ -901,7 +901,7 @@ AABox HeightFieldShape::GetLocalBounds() const
 }
 
 #ifdef JPH_DEBUG_RENDERER
-void HeightFieldShape::Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+void HeightFieldShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
 {
 	// Don't draw anything if we don't have any collision
 	if (mHeightSamples.empty())
@@ -983,7 +983,7 @@ void HeightFieldShape::Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTr
 	}
 
 	// Get transform including scale
-	Mat44 transform = inCenterOfMassTransform * Mat44::sScale(inScale);
+	RMat44 transform = inCenterOfMassTransform.PreScaled(inScale);
 
 	// Test if the shape is scaled inside out
 	DebugRenderer::ECullMode cull_mode = ScaleHelpers::IsInsideOut(inScale)? DebugRenderer::ECullMode::CullFrontFace : DebugRenderer::ECullMode::CullBackFace;
@@ -999,7 +999,7 @@ void HeightFieldShape::Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTr
 	{
 		struct Visitor
 		{
-			JPH_INLINE explicit		Visitor(const HeightFieldShape *inShape, DebugRenderer *inRenderer, Mat44Arg inTransform) :
+			JPH_INLINE explicit		Visitor(const HeightFieldShape *inShape, DebugRenderer *inRenderer, RMat44Arg inTransform) :
 				mShape(inShape),
 				mRenderer(inRenderer),
 				mTransform(inTransform)
@@ -1031,8 +1031,8 @@ void HeightFieldShape::Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTr
 				Vec3 v[] = { inV0, inV1, inV2 };
 				for (uint edge_idx = 0; edge_idx < 3; ++edge_idx)
 				{
-					Vec3 v1 = mTransform * v[edge_idx];
-					Vec3 v2 = mTransform * v[(edge_idx + 1) % 3];
+					RVec3 v1 = mTransform * v[edge_idx];
+					RVec3 v2 = mTransform * v[(edge_idx + 1) % 3];
 
 					// Draw active edge as a green arrow, other edges as grey
 					if (active_edges & (1 << edge_idx))
@@ -1044,10 +1044,10 @@ void HeightFieldShape::Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTr
 
 			const HeightFieldShape *mShape;
 			DebugRenderer *			mRenderer;
-			Mat44					mTransform;
+			RMat44					mTransform;
 		};
 
-		Visitor visitor(this, inRenderer, inCenterOfMassTransform * Mat44::sScale(inScale));
+		Visitor visitor(this, inRenderer, inCenterOfMassTransform.PreScaled(inScale));
 		WalkHeightField(visitor);
 	}
 }

Jolt/Physics/Collision/Shape/HeightFieldShape.h

@@ -125,11 +125,11 @@ public:
 	virtual void					GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override;
 
 	// See Shape::GetSubmergedVolume
-	virtual void					GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy) const override { JPH_ASSERT(false, "Not supported"); }
+	virtual void					GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override { JPH_ASSERT(false, "Not supported"); }
 
 #ifdef JPH_DEBUG_RENDERER
 	// See Shape::Draw
-	virtual void					Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+	virtual void					Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
 #endif // JPH_DEBUG_RENDERER
 
 	// See Shape::CastRay

Jolt/Physics/Collision/Shape/MeshShape.cpp

@@ -496,7 +496,7 @@ JPH_INLINE void MeshShape::WalkTreePerTriangle(const SubShapeIDCreator &inSubSha
 }
 
 #ifdef JPH_DEBUG_RENDERER
-void MeshShape::Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+void MeshShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
 {
 	// Reset the batch if we switch coloring mode
 	if (mCachedTrianglesColoredPerGroup != sDrawTriangleGroups || mCachedUseMaterialColors != inUseMaterialColors)
@@ -577,7 +577,7 @@ void MeshShape::Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform
 	{
 		struct Visitor
 		{
-			JPH_INLINE 			Visitor(DebugRenderer *inRenderer, Mat44Arg inTransform) :
+			JPH_INLINE 			Visitor(DebugRenderer *inRenderer, RMat44Arg inTransform) :
 				mRenderer(inRenderer),
 				mTransform(inTransform)
 			{
@@ -614,8 +614,8 @@ void MeshShape::Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform
 					// Loop through edges
 					for (uint edge_idx = 0; edge_idx < 3; ++edge_idx)
 					{
-						Vec3 v1 = mTransform * v[edge_idx];
-						Vec3 v2 = mTransform * v[(edge_idx + 1) % 3];
+						RVec3 v1 = mTransform * v[edge_idx];
+						RVec3 v2 = mTransform * v[(edge_idx + 1) % 3];
 
 						// Draw active edge as a green arrow, other edges as grey
 						if (*f & (1 << (edge_idx + FLAGS_ACTIVE_EGDE_SHIFT)))
@@ -627,10 +627,10 @@ void MeshShape::Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform
 			}
 
 			DebugRenderer *	mRenderer;
-			Mat44			mTransform;
+			RMat44			mTransform;
 		};
 
-		Visitor visitor { inRenderer, inCenterOfMassTransform * Mat44::sScale(inScale) };
+		Visitor visitor { inRenderer, inCenterOfMassTransform.PreScaled(inScale) };
 		WalkTree(visitor);
 	}
 }

Jolt/Physics/Collision/Shape/MeshShape.h

@@ -91,7 +91,7 @@ public:
 
 #ifdef JPH_DEBUG_RENDERER
 	// See Shape::Draw
-	virtual void					Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+	virtual void					Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
 #endif // JPH_DEBUG_RENDERER
 
 	// See Shape::CastRay
@@ -110,7 +110,7 @@ public:
 	virtual int						GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override;
 
 	// See Shape::GetSubmergedVolume
-	virtual void					GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy) const override { JPH_ASSERT(false, "Not supported"); }
+	virtual void					GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override { JPH_ASSERT(false, "Not supported"); }
 
 	// See Shape
 	virtual void					SaveBinaryState(StreamOut &inStream) const override;

Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.cpp

@@ -56,7 +56,7 @@ TransformedShape OffsetCenterOfMassShape::GetSubShapeTransformedShape(const SubS
 	// We don't use any bits in the sub shape ID
 	outRemainder = inSubShapeID;
 
-	TransformedShape ts(inPositionCOM - inRotation * mOffset, inRotation, mInnerShape, BodyID());
+	TransformedShape ts(RVec3(inPositionCOM - inRotation * mOffset), inRotation, mInnerShape, BodyID());
 	ts.SetShapeScale(inScale);
 	return ts;
 }
@@ -72,23 +72,23 @@ void OffsetCenterOfMassShape::GetSupportingFace(const SubShapeID &inSubShapeID,
 	mInnerShape->GetSupportingFace(inSubShapeID, inDirection, inScale, inCenterOfMassTransform.PreTranslated(-mOffset), outVertices);
 }
 
-void OffsetCenterOfMassShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy) const
+void OffsetCenterOfMassShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const
 {
-	mInnerShape->GetSubmergedVolume(inCenterOfMassTransform.PreTranslated(-mOffset), inScale, inSurface, outTotalVolume, outSubmergedVolume, outCenterOfBuoyancy);
+	mInnerShape->GetSubmergedVolume(inCenterOfMassTransform.PreTranslated(-mOffset), inScale, inSurface, outTotalVolume, outSubmergedVolume, outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, inBaseOffset));
 }
 
 #ifdef JPH_DEBUG_RENDERER
-void OffsetCenterOfMassShape::Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+void OffsetCenterOfMassShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
 {
 	mInnerShape->Draw(inRenderer, inCenterOfMassTransform.PreTranslated(-mOffset), inScale, inColor, inUseMaterialColors, inDrawWireframe);
 }
 
-void OffsetCenterOfMassShape::DrawGetSupportFunction(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const
+void OffsetCenterOfMassShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const
 {
 	mInnerShape->DrawGetSupportFunction(inRenderer, inCenterOfMassTransform.PreTranslated(-mOffset), inScale, inColor, inDrawSupportDirection);
 }
 
-void OffsetCenterOfMassShape::DrawGetSupportingFace(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
+void OffsetCenterOfMassShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
 {
 	mInnerShape->DrawGetSupportingFace(inRenderer, inCenterOfMassTransform.PreTranslated(-mOffset), inScale);
 }

Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.h

@@ -51,6 +51,7 @@ public:
 		
 	// See Shape::GetWorldSpaceBounds
 	virtual AABox					GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
+	using Shape::GetWorldSpaceBounds;
 
 	// See Shape::GetInnerRadius
 	virtual float					GetInnerRadius() const override							{ return mInnerShape->GetInnerRadius(); }
@@ -68,17 +69,17 @@ public:
 	virtual void					GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override;
 
 	// See Shape::GetSubmergedVolume
-	virtual void					GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy) const override;
+	virtual void					GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override;
 
 #ifdef JPH_DEBUG_RENDERER
 	// See Shape::Draw
-	virtual void					Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+	virtual void					Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
 
 	// See Shape::DrawGetSupportFunction
-	virtual void					DrawGetSupportFunction(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const override;
+	virtual void					DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const override;
 
 	// See Shape::DrawGetSupportingFace
-	virtual void					DrawGetSupportingFace(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
+	virtual void					DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
 #endif // JPH_DEBUG_RENDERER
 
 	// See Shape::CastRay

Jolt/Physics/Collision/Shape/PolyhedronSubmergedVolumeCalculator.h

@@ -41,7 +41,7 @@ private:
 	// Calculate submerged volume * 6 and center of mass * 4 for a tetrahedron with 1 vertex submerged
 	// inV1 is submerged, inV2 .. inV4 are not
 	// inD1 .. inD4 are the distances from the points to the plane
-	inline static void	sTetrahedronVolume1(Vec3Arg inV1, float inD1, Vec3Arg inV2, float inD2, Vec3Arg inV3, float inD3, Vec3Arg inV4, float inD4, float &outVolumeTimes6, Vec3 &outCenterTimes4)
+	inline JPH_IF_NOT_DEBUG_RENDERER(static) void sTetrahedronVolume1(Vec3Arg inV1, float inD1, Vec3Arg inV2, float inD2, Vec3Arg inV3, float inD3, Vec3Arg inV4, float inD4, float &outVolumeTimes6, Vec3 &outCenterTimes4)
 	{
 		// A tetrahedron with 1 point submerged is cut along 3 edges forming a new tetrahedron
 		Vec3 v2 = sGetPlaneIntersection(inV1, inD1, inV2, inD2);
@@ -52,8 +52,12 @@ private:
 		// Draw intersection between tetrahedron and surface
 		if (Shape::sDrawSubmergedVolumes)
 		{
-			DebugRenderer::sInstance->DrawTriangle(v4, v3, v2, Color::sGreen);
-			DebugRenderer::sInstance->DrawWireTriangle(v4, v3, v2, Color::sWhite);
+			RVec3 v2w = mBaseOffset + v2;
+			RVec3 v3w = mBaseOffset + v3;
+			RVec3 v4w = mBaseOffset + v4;
+
+			DebugRenderer::sInstance->DrawTriangle(v4w, v3w, v2w, Color::sGreen);
+			DebugRenderer::sInstance->DrawWireTriangle(v4w, v3w, v2w, Color::sWhite);
 		}
 	#endif // JPH_DEBUG_RENDERER
 
@@ -63,7 +67,7 @@ private:
 	// Calculate submerged volume * 6 and center of mass * 4 for a tetrahedron with 2 vertices submerged
 	// inV1, inV2 are submerged, inV3, inV4 are not
 	// inD1 .. inD4 are the distances from the points to the plane
-	inline static void	sTetrahedronVolume2(Vec3Arg inV1, float inD1, Vec3Arg inV2, float inD2, Vec3Arg inV3, float inD3, Vec3Arg inV4, float inD4, float &outVolumeTimes6, Vec3 &outCenterTimes4)
+	inline JPH_IF_NOT_DEBUG_RENDERER(static) void sTetrahedronVolume2(Vec3Arg inV1, float inD1, Vec3Arg inV2, float inD2, Vec3Arg inV3, float inD3, Vec3Arg inV4, float inD4, float &outVolumeTimes6, Vec3 &outCenterTimes4)
 	{
 		// A tetrahedron with 2 points submerged is cut along 4 edges forming a quad
 		Vec3 c = sGetPlaneIntersection(inV1, inD1, inV3, inD3);
@@ -75,10 +79,15 @@ private:
 		// Draw intersection between tetrahedron and surface
 		if (Shape::sDrawSubmergedVolumes)
 		{
-			DebugRenderer::sInstance->DrawTriangle(c, e, d, Color::sGreen);
-			DebugRenderer::sInstance->DrawTriangle(c, f, e, Color::sGreen);
-			DebugRenderer::sInstance->DrawWireTriangle(c, e, d, Color::sWhite);
-			DebugRenderer::sInstance->DrawWireTriangle(c, f, e, Color::sWhite);
+			RVec3 cw = mBaseOffset + c;
+			RVec3 dw = mBaseOffset + d;
+			RVec3 ew = mBaseOffset + e;
+			RVec3 fw = mBaseOffset + f;
+
+			DebugRenderer::sInstance->DrawTriangle(cw, ew, dw, Color::sGreen);
+			DebugRenderer::sInstance->DrawTriangle(cw, fw, ew, Color::sGreen);
+			DebugRenderer::sInstance->DrawWireTriangle(cw, ew, dw, Color::sWhite);
+			DebugRenderer::sInstance->DrawWireTriangle(cw, fw, ew, Color::sWhite);
 		}
 	#endif // JPH_DEBUG_RENDERER
 
@@ -98,7 +107,7 @@ private:
 	// Calculate submerged volume * 6 and center of mass * 4 for a tetrahedron with 3 vertices submerged
 	// inV1, inV2, inV3 are submerged, inV4 is not
 	// inD1 .. inD4 are the distances from the points to the plane
-	inline static void	sTetrahedronVolume3(Vec3Arg inV1, float inD1, Vec3Arg inV2, float inD2, Vec3Arg inV3, float inD3, Vec3Arg inV4, float inD4, float &outVolumeTimes6, Vec3 &outCenterTimes4)
+	inline JPH_IF_NOT_DEBUG_RENDERER(static) void sTetrahedronVolume3(Vec3Arg inV1, float inD1, Vec3Arg inV2, float inD2, Vec3Arg inV3, float inD3, Vec3Arg inV4, float inD4, float &outVolumeTimes6, Vec3 &outCenterTimes4)
 	{
 		// A tetrahedron with 1 point above the surface is cut along 3 edges forming a new tetrahedron
 		Vec3 v1 = sGetPlaneIntersection(inV1, inD1, inV4, inD4);
@@ -109,8 +118,12 @@ private:
 		// Draw intersection between tetrahedron and surface
 		if (Shape::sDrawSubmergedVolumes)
 		{
-			DebugRenderer::sInstance->DrawTriangle(v3, v2, v1, Color::sGreen);
-			DebugRenderer::sInstance->DrawWireTriangle(v3, v2, v1, Color::sWhite);
+			RVec3 v1w = mBaseOffset + v1;
+			RVec3 v2w = mBaseOffset + v2;
+			RVec3 v3w = mBaseOffset + v3;
+
+			DebugRenderer::sInstance->DrawTriangle(v3w, v2w, v1w, Color::sGreen);
+			DebugRenderer::sInstance->DrawWireTriangle(v3w, v2w, v1w, Color::sWhite);
 		}
 	#endif // JPH_DEBUG_RENDERER
 
@@ -145,8 +158,16 @@ public:
 	/// @param inNumPoints The amount of points
 	/// @param inSurface The plane that forms the fluid surface (normal should point up)
 	/// @param ioBuffer A temporary buffer of Point's that should have inNumPoints entries and should stay alive while this class is alive
-						PolyhedronSubmergedVolumeCalculator(const Mat44 &inTransform, const Vec3 *inPoints, int inPointStride, int inNumPoints, const Plane &inSurface, Point *ioBuffer) :
+	/// @param inBaseOffset The offset to transform inTransform to world space (in double precision mode this can be used to shift the whole operation closer to the origin). Only used for debug drawing.
+						PolyhedronSubmergedVolumeCalculator(const Mat44 &inTransform, const Vec3 *inPoints, int inPointStride, int inNumPoints, const Plane &inSurface, Point *ioBuffer
+#ifdef JPH_DEBUG_RENDERER // Not using JPH_IF_DEBUG_RENDERER for Doxygen
+		, RVec3 inBaseOffset
+#endif // JPH_DEBUG_RENDERER
+		) :
 		mPoints(ioBuffer)
+#ifdef JPH_DEBUG_RENDERER
+		, mBaseOffset(inBaseOffset)
+#endif // JPH_DEBUG_RENDERER
 	{
 		// Convert the points to world space and determine the distance to the surface
 		float reference_dist = FLT_MAX;
@@ -285,6 +306,11 @@ private:
 	// Aggregator for submerged volume and center of buoyancy
 	float				mSubmergedVolume = 0.0f;
 	Vec3				mCenterOfBuoyancy = Vec3::sZero();
+
+#ifdef JPH_DEBUG_RENDERER
+	// Base offset used for drawing
+	RVec3				mBaseOffset;
+#endif
 };
 
 JPH_NAMESPACE_END

Jolt/Physics/Collision/Shape/RotatedTranslatedShape.cpp

@@ -80,7 +80,7 @@ TransformedShape RotatedTranslatedShape::GetSubShapeTransformedShape(const SubSh
 	// We don't use any bits in the sub shape ID
 	outRemainder = inSubShapeID;
 
-	TransformedShape ts(inPositionCOM, inRotation * mRotation, mInnerShape, BodyID());
+	TransformedShape ts(RVec3(inPositionCOM), inRotation * mRotation, mInnerShape, BodyID());
 	ts.SetShapeScale(TransformScale(inScale));
 	return ts;
 }
@@ -101,27 +101,27 @@ void RotatedTranslatedShape::GetSupportingFace(const SubShapeID &inSubShapeID, V
 	mInnerShape->GetSupportingFace(inSubShapeID, transform.Multiply3x3Transposed(inDirection), TransformScale(inScale), inCenterOfMassTransform * transform, outVertices);
 }
 
-void RotatedTranslatedShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy) const
+void RotatedTranslatedShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const
 {
 	// Get center of mass transform of child
 	Mat44 transform = inCenterOfMassTransform * Mat44::sRotation(mRotation);
 
 	// Recurse to child
-	mInnerShape->GetSubmergedVolume(transform, TransformScale(inScale), inSurface, outTotalVolume, outSubmergedVolume, outCenterOfBuoyancy);
+	mInnerShape->GetSubmergedVolume(transform, TransformScale(inScale), inSurface, outTotalVolume, outSubmergedVolume, outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, inBaseOffset));
 }
 
 #ifdef JPH_DEBUG_RENDERER
-void RotatedTranslatedShape::Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+void RotatedTranslatedShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
 {
 	mInnerShape->Draw(inRenderer, inCenterOfMassTransform * Mat44::sRotation(mRotation), TransformScale(inScale), inColor, inUseMaterialColors, inDrawWireframe);
 }
 
-void RotatedTranslatedShape::DrawGetSupportFunction(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const
+void RotatedTranslatedShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const
 {
 	mInnerShape->DrawGetSupportFunction(inRenderer, inCenterOfMassTransform * Mat44::sRotation(mRotation), TransformScale(inScale), inColor, inDrawSupportDirection);
 }
 
-void RotatedTranslatedShape::DrawGetSupportingFace(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
+void RotatedTranslatedShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
 {
 	mInnerShape->DrawGetSupportingFace(inRenderer, inCenterOfMassTransform * Mat44::sRotation(mRotation), TransformScale(inScale));
 }

Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h

@@ -58,6 +58,7 @@ public:
 		
 	// See Shape::GetWorldSpaceBounds
 	virtual AABox					GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
+	using Shape::GetWorldSpaceBounds;
 
 	// See Shape::GetInnerRadius
 	virtual float					GetInnerRadius() const override							{ return mInnerShape->GetInnerRadius(); }
@@ -75,17 +76,17 @@ public:
 	virtual void					GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override;
 
 	// See Shape::GetSubmergedVolume
-	virtual void					GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy) const override;
+	virtual void					GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override;
 
 #ifdef JPH_DEBUG_RENDERER
 	// See Shape::Draw
-	virtual void					Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+	virtual void					Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
 
 	// See Shape::DrawGetSupportFunction
-	virtual void					DrawGetSupportFunction(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const override;
+	virtual void					DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const override;
 
 	// See Shape::DrawGetSupportingFace
-	virtual void					DrawGetSupportingFace(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
+	virtual void					DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
 #endif // JPH_DEBUG_RENDERER
 
 	// See Shape::CastRay

Jolt/Physics/Collision/Shape/ScaledShape.cpp

@@ -60,7 +60,7 @@ TransformedShape ScaledShape::GetSubShapeTransformedShape(const SubShapeID &inSu
 	// We don't use any bits in the sub shape ID
 	outRemainder = inSubShapeID;
 
-	TransformedShape ts(inPositionCOM, inRotation, mInnerShape, BodyID());
+	TransformedShape ts(RVec3(inPositionCOM), inRotation, mInnerShape, BodyID());
 	ts.SetShapeScale(inScale * mScale);
 	return ts;
 }
@@ -81,23 +81,23 @@ void ScaledShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDi
 	mInnerShape->GetSupportingFace(inSubShapeID, inDirection, inScale * mScale, inCenterOfMassTransform, outVertices);
 }
 
-void ScaledShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy) const
+void ScaledShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const
 {
-	mInnerShape->GetSubmergedVolume(inCenterOfMassTransform, inScale * mScale, inSurface, outTotalVolume, outSubmergedVolume, outCenterOfBuoyancy);
+	mInnerShape->GetSubmergedVolume(inCenterOfMassTransform, inScale * mScale, inSurface, outTotalVolume, outSubmergedVolume, outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, inBaseOffset));
 }
 
 #ifdef JPH_DEBUG_RENDERER
-void ScaledShape::Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+void ScaledShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
 {
 	mInnerShape->Draw(inRenderer, inCenterOfMassTransform, inScale * mScale, inColor, inUseMaterialColors, inDrawWireframe);
 }
 
-void ScaledShape::DrawGetSupportFunction(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const
+void ScaledShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const
 {
 	mInnerShape->DrawGetSupportFunction(inRenderer, inCenterOfMassTransform, inScale * mScale, inColor, inDrawSupportDirection);
 }
 
-void ScaledShape::DrawGetSupportingFace(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const 
+void ScaledShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const 
 { 
 	mInnerShape->DrawGetSupportingFace(inRenderer, inCenterOfMassTransform, inScale * mScale);
 }

Jolt/Physics/Collision/Shape/ScaledShape.h

@@ -51,6 +51,7 @@ public:
 		
 	// See Shape::GetWorldSpaceBounds
 	virtual AABox					GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
+	using Shape::GetWorldSpaceBounds;
 
 	// See Shape::GetInnerRadius
 	virtual float					GetInnerRadius() const override							{ return mScale.ReduceMin() * mInnerShape->GetInnerRadius(); }
@@ -68,17 +69,17 @@ public:
 	virtual void					GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override;
 
 	// See Shape::GetSubmergedVolume
-	virtual void					GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy) const override;
+	virtual void					GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override;
 
 #ifdef JPH_DEBUG_RENDERER
 	// See Shape::Draw
-	virtual void					Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+	virtual void					Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
 
 	// See Shape::DrawGetSupportFunction
-	virtual void					DrawGetSupportFunction(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const override;
+	virtual void					DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const override;
 
 	// See Shape::DrawGetSupportingFace
-	virtual void					DrawGetSupportingFace(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
+	virtual void					DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
 #endif // JPH_DEBUG_RENDERER
 
 	// See Shape::CastRay

Jolt/Physics/Collision/Shape/Shape.cpp

@@ -34,7 +34,7 @@ TransformedShape Shape::GetSubShapeTransformedShape(const SubShapeID &inSubShape
 	outRemainder = SubShapeID();
 
 	// Just return the transformed shape for this shape
-	TransformedShape ts(inPositionCOM, inRotation, this, BodyID());
+	TransformedShape ts(RVec3(inPositionCOM), inRotation, this, BodyID());
 	ts.SetShapeScale(inScale);
 	return ts;
 }
@@ -45,7 +45,7 @@ void Shape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM,
 	if (!inShapeFilter.ShouldCollide(inSubShapeIDCreator.GetID()))
 		return;
 
-	TransformedShape ts(inPositionCOM, inRotation, this, TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator);
+	TransformedShape ts(RVec3(inPositionCOM), inRotation, this, TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator);
 	ts.SetShapeScale(inScale);
 	ioCollector.AddHit(ts);
 }
@@ -54,7 +54,7 @@ void Shape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCol
 {
 	Vec3 scale;
 	Mat44 transform = inCenterOfMassTransform.Decompose(scale);
-	TransformedShape ts(transform.GetTranslation(), transform.GetRotation().GetQuaternion(), this, BodyID(), SubShapeIDCreator());
+	TransformedShape ts(RVec3(transform.GetTranslation()), transform.GetRotation().GetQuaternion(), this, BodyID(), SubShapeIDCreator());
 	ts.SetShapeScale(scale);
 	ioCollector.AddHit(ts);
 }
@@ -320,7 +320,7 @@ Shape::ShapeResult Shape::ScaleShape(Vec3Arg inScale) const
 			shape = new ScaledShape(shape, scale);
 
 		// Add the shape
-		compound.AddShape(ts.mShapePositionCOM - ts.mShapeRotation * shape->GetCenterOfMass(), ts.mShapeRotation, shape);
+		compound.AddShape(Vec3(ts.mShapePositionCOM) - ts.mShapeRotation * shape->GetCenterOfMass(), ts.mShapeRotation, shape);
 	}
 
 	return compound.Create();

Jolt/Physics/Collision/Shape/Shape.h

@@ -194,6 +194,9 @@ public:
 	/// This function can be overridden to return a closer fitting world space bounding box, by default it will just transform what GetLocalBounds() returns.
 	virtual AABox					GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const { return GetLocalBounds().Scaled(inScale).Transformed(inCenterOfMassTransform); }
 
+	/// Get world space bounds including convex radius.
+	AABox							GetWorldSpaceBounds(DMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const { return GetLocalBounds().Scaled(inScale).Transformed(inCenterOfMassTransform); }
+
 	/// Returns the radius of the biggest sphere that fits entirely in the shape. In case this shape consists of multiple sub shapes, it returns the smallest sphere of the parts. 
 	/// This can be used as a measure of how far the shape can be moved without risking going through geometry.
 	virtual float					GetInnerRadius() const = 0;
@@ -233,23 +236,28 @@ public:
 	virtual TransformedShape		GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const;
 
 	/// Gets the properties needed to do buoyancy calculations for a body using this shape
-	/// @param inCenterOfMassTransform Transform that takes this shape (centered around center of mass) to world space
+	/// @param inCenterOfMassTransform Transform that takes this shape (centered around center of mass) to world space (or a desired other space)
 	/// @param inScale Scale in local space of the shape
-	/// @param inSurface The surface plane of the liquid in world space
+	/// @param inSurface The surface plane of the liquid relative to inCenterOfMassTransform
 	/// @param outTotalVolume On return this contains the total volume of the shape
 	/// @param outSubmergedVolume On return this contains the submerged volume of the shape
 	/// @param outCenterOfBuoyancy On return this contains the world space center of mass of the submerged volume
-	virtual void					GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy) const = 0;
+	/// @param inBaseOffset The offset to transform inCenterOfMassTransform to world space (in double precision mode this can be used to shift the whole operation closer to the origin). Only used for debug drawing.
+	virtual void					GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy
+#ifdef JPH_DEBUG_RENDERER // Not using JPH_IF_DEBUG_RENDERER for Doxygen
+		, RVec3Arg inBaseOffset
+#endif
+		) const = 0;
 	
 #ifdef JPH_DEBUG_RENDERER
 	/// Draw the shape at a particular location with a particular color (debugging purposes)
-	virtual void					Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const = 0;
+	virtual void					Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const = 0;
 
 	/// Draw the results of the GetSupportFunction with the convex radius added back on to show any errors introduced by this process (only relevant for convex shapes)
-	virtual void					DrawGetSupportFunction(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const { /* Only implemented for convex shapes */ }
+	virtual void					DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const { /* Only implemented for convex shapes */ }
 
 	/// Draw the results of the GetSupportingFace function to show any errors introduced by this process (only relevant for convex shapes)
-	virtual void					DrawGetSupportingFace(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const { /* Only implemented for convex shapes */ }
+	virtual void					DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const { /* Only implemented for convex shapes */ }
 #endif // JPH_DEBUG_RENDERER
 
 	/// Cast a ray against this shape, returns true if it finds a hit closer than ioHit.mFraction and updates that fraction. Otherwise ioHit is left untouched and the function returns false.

Jolt/Physics/Collision/Shape/SphereShape.cpp

@@ -160,7 +160,7 @@ Vec3 SphereShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLoc
 	return len != 0.0f? inLocalSurfacePosition / len : Vec3::sAxisY();
 }
 
-void SphereShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy) const
+void SphereShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const
 {
 	float scaled_radius = GetScaledRadius(inScale);
 	outTotalVolume = (4.0f / 3.0f * JPH_PI) * Cubed(scaled_radius);
@@ -196,7 +196,7 @@ void SphereShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg i
 		{
 			Vec3 circle_center = inCenterOfMassTransform.GetTranslation() - distance_to_surface * inSurface.GetNormal();
 			float circle_radius = sqrt(Square(scaled_radius) - Square(distance_to_surface));
-			DebugRenderer::sInstance->DrawPie(circle_center, circle_radius, inSurface.GetNormal(), inSurface.GetNormal().GetNormalizedPerpendicular(), -JPH_PI, JPH_PI, Color::sGreen, DebugRenderer::ECastShadow::Off);
+			DebugRenderer::sInstance->DrawPie(inBaseOffset + circle_center, circle_radius, inSurface.GetNormal(), inSurface.GetNormal().GetNormalizedPerpendicular(), -JPH_PI, JPH_PI, Color::sGreen, DebugRenderer::ECastShadow::Off);
 		}
 	#endif // JPH_DEBUG_RENDERER
 	}
@@ -204,12 +204,12 @@ void SphereShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg i
 #ifdef JPH_DEBUG_RENDERER
 	// Draw center of buoyancy
 	if (sDrawSubmergedVolumes)
-		DebugRenderer::sInstance->DrawWireSphere(outCenterOfBuoyancy, 0.05f, Color::sRed, 1);
+		DebugRenderer::sInstance->DrawWireSphere(inBaseOffset + outCenterOfBuoyancy, 0.05f, Color::sRed, 1);
 #endif // JPH_DEBUG_RENDERER
 }
 
 #ifdef JPH_DEBUG_RENDERER
-void SphereShape::Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+void SphereShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
 {
 	DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid;
 	inRenderer->DrawUnitSphere(inCenterOfMassTransform * Mat44::sScale(mRadius * inScale.Abs().GetX()), inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor, DebugRenderer::ECastShadow::On, draw_mode);
@@ -277,7 +277,7 @@ void SphereShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedSh
 {
 	Vec3 scale;
 	Mat44 transform = inCenterOfMassTransform.Decompose(scale);
-	TransformedShape ts(transform.GetTranslation(), transform.GetRotation().GetQuaternion(), this, BodyID(), SubShapeIDCreator());
+	TransformedShape ts(RVec3(transform.GetTranslation()), transform.GetRotation().GetQuaternion(), this, BodyID(), SubShapeIDCreator());
 	ts.SetShapeScale(ScaleHelpers::MakeUniformScale(scale.Abs()));
 	ioCollector.AddHit(ts);
 }

Jolt/Physics/Collision/Shape/SphereShape.h

@@ -47,6 +47,7 @@ public:
 		
 	// See Shape::GetWorldSpaceBounds
 	virtual AABox			GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
+	using Shape::GetWorldSpaceBounds;
 
 	// See Shape::GetInnerRadius
 	virtual float			GetInnerRadius() const override														{ return mRadius; }
@@ -64,11 +65,11 @@ public:
 	virtual const Support *	GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override;
 
 	// See Shape::GetSubmergedVolume
-	virtual void			GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy) const override;
+	virtual void			GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override;
 
 #ifdef JPH_DEBUG_RENDERER
 	// See Shape::Draw
-	virtual void			Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+	virtual void			Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
 #endif // JPH_DEBUG_RENDERER
 
 	// See Shape::CastRay

Jolt/Physics/Collision/Shape/TaperedCapsuleShape.cpp

@@ -299,7 +299,7 @@ AABox TaperedCapsuleShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform,
 }
 
 #ifdef JPH_DEBUG_RENDERER
-void TaperedCapsuleShape::Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+void TaperedCapsuleShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
 {
 	if (mGeometry == nullptr)
 	{
@@ -310,9 +310,9 @@ void TaperedCapsuleShape::Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMas
 
 	// Preserve flip along y axis but make sure we're not inside out
 	Vec3 scale = ScaleHelpers::IsInsideOut(inScale)? Vec3(-1, 1, 1) * inScale : inScale;
-	Mat44 world_transform = inCenterOfMassTransform * Mat44::sScale(scale);
+	RMat44 world_transform = inCenterOfMassTransform * Mat44::sScale(scale);
 
-	AABox bounds = GetWorldSpaceBounds(inCenterOfMassTransform, inScale);
+	AABox bounds = Shape::GetWorldSpaceBounds(inCenterOfMassTransform, inScale);
 
 	float lod_scale_sq = Square(max(mTopRadius, mBottomRadius));
 
@@ -335,7 +335,7 @@ void TaperedCapsuleShape::TransformShape(Mat44Arg inCenterOfMassTransform, Trans
 {
 	Vec3 scale;
 	Mat44 transform = inCenterOfMassTransform.Decompose(scale);
-	TransformedShape ts(transform.GetTranslation(), transform.GetRotation().GetQuaternion(), this, BodyID(), SubShapeIDCreator());
+	TransformedShape ts(RVec3(transform.GetTranslation()), transform.GetRotation().GetQuaternion(), this, BodyID(), SubShapeIDCreator());
 	ts.SetShapeScale(scale.GetSign() * ScaleHelpers::MakeUniformScale(scale.Abs()));
 	ioCollector.AddHit(ts);
 }

Jolt/Physics/Collision/Shape/TaperedCapsuleShape.h

@@ -53,6 +53,7 @@ public:
 
 	// See Shape::GetWorldSpaceBounds
 	virtual AABox			GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
+	using Shape::GetWorldSpaceBounds;
 
 	// See Shape::GetInnerRadius
 	virtual float			GetInnerRadius() const override											{ return min(mTopRadius, mBottomRadius); }
@@ -71,7 +72,7 @@ public:
 
 #ifdef JPH_DEBUG_RENDERER
 	// See Shape::Draw
-	virtual void			Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+	virtual void			Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
 #endif // JPH_DEBUG_RENDERER
 
 	// See Shape::TransformShape

Jolt/Physics/Collision/Shape/TriangleShape.cpp

@@ -192,7 +192,7 @@ Vec3 TriangleShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inL
 	return len != 0.0f? cross / len : Vec3::sAxisY();
 }
 
-void TriangleShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy) const
+void TriangleShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const
 {
 	// A triangle has no volume
 	outTotalVolume = outSubmergedVolume = 0.0f;
@@ -200,11 +200,11 @@ void TriangleShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg
 }
 
 #ifdef JPH_DEBUG_RENDERER
-void TriangleShape::Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
+void TriangleShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
 {
-	Vec3 v1 = inCenterOfMassTransform * (inScale * mV1);
-	Vec3 v2 = inCenterOfMassTransform * (inScale * mV2);
-	Vec3 v3 = inCenterOfMassTransform * (inScale * mV3);
+	RVec3 v1 = inCenterOfMassTransform * (inScale * mV1);
+	RVec3 v2 = inCenterOfMassTransform * (inScale * mV2);
+	RVec3 v3 = inCenterOfMassTransform * (inScale * mV3);
 
 	if (ScaleHelpers::IsInsideOut(inScale))
 		swap(v1, v2);
@@ -300,7 +300,7 @@ void TriangleShape::TransformShape(Mat44Arg inCenterOfMassTransform, Transformed
 {
 	Vec3 scale;
 	Mat44 transform = inCenterOfMassTransform.Decompose(scale);
-	TransformedShape ts(transform.GetTranslation(), transform.GetRotation().GetQuaternion(), this, BodyID(), SubShapeIDCreator());
+	TransformedShape ts(RVec3(transform.GetTranslation()), transform.GetRotation().GetQuaternion(), this, BodyID(), SubShapeIDCreator());
 	ts.SetShapeScale(mConvexRadius == 0.0f? scale : scale.GetSign() * ScaleHelpers::MakeUniformScale(scale.Abs()));
 	ioCollector.AddHit(ts);
 }

Jolt/Physics/Collision/Shape/TriangleShape.h

@@ -51,6 +51,7 @@ public:
 		
 	// See Shape::GetWorldSpaceBounds
 	virtual AABox			GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override;
+	using Shape::GetWorldSpaceBounds;
 
 	// See Shape::GetInnerRadius
 	virtual float			GetInnerRadius() const override														{ return mConvexRadius; }
@@ -68,11 +69,11 @@ public:
 	virtual const Support *	GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override;
 
 	// See Shape::GetSubmergedVolume
-	virtual void			GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy) const override;
+	virtual void			GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override;
 
 #ifdef JPH_DEBUG_RENDERER
 	// See Shape::Draw
-	virtual void			Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
+	virtual void			Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override;
 #endif // JPH_DEBUG_RENDERER
 
 	// See Shape::CastRay

Jolt/Physics/Collision/ShapeCast.h

@@ -10,12 +10,13 @@
 JPH_NAMESPACE_BEGIN
 
 /// Structure that holds a single shape cast (a shape moving along a linear path in 3d space with no rotation)
-struct ShapeCast
+template <class Vec, class Mat, class ShapeCastType>
+struct ShapeCastT
 {
 	JPH_OVERRIDE_NEW_DELETE
 
 	/// Constructor
-								ShapeCast(const Shape *inShape, Vec3Arg inScale, Mat44Arg inCenterOfMassStart, Vec3Arg inDirection, const AABox &inWorldSpaceBounds) :
+								ShapeCastT(const Shape *inShape, Vec3Arg inScale, typename Mat::ArgType inCenterOfMassStart, Vec3Arg inDirection, const AABox &inWorldSpaceBounds) :
 		mShape(inShape),
 		mScale(inScale),
 		mCenterOfMassStart(inCenterOfMassStart),
@@ -25,32 +26,66 @@ struct ShapeCast
 	}
 
 	/// Constructor
-								ShapeCast(const Shape *inShape, Vec3Arg inScale, Mat44Arg inCenterOfMassStart, Vec3Arg inDirection) :
-		ShapeCast(inShape, inScale, inCenterOfMassStart, inDirection, inShape->GetWorldSpaceBounds(inCenterOfMassStart, inScale))
+								ShapeCastT(const Shape *inShape, Vec3Arg inScale, typename Mat::ArgType inCenterOfMassStart, Vec3Arg inDirection) :
+		ShapeCastT<Vec, Mat, ShapeCastType>(inShape, inScale, inCenterOfMassStart, inDirection, inShape->GetWorldSpaceBounds(inCenterOfMassStart, inScale))
 	{
 	}
 
 	/// Construct a shape cast using a world transform for a shape instead of a center of mass transform
-	static inline ShapeCast		sFromWorldTransform(const Shape *inShape, Vec3Arg inScale, Mat44Arg inWorldTransform, Vec3Arg inDirection)
+	static inline ShapeCastType	sFromWorldTransform(const Shape *inShape, Vec3Arg inScale, typename Mat::ArgType inWorldTransform, Vec3Arg inDirection)
 	{
-		return ShapeCast(inShape, inScale, inWorldTransform.PreTranslated(inShape->GetCenterOfMass()), inDirection);
+		return ShapeCastType(inShape, inScale, inWorldTransform.PreTranslated(inShape->GetCenterOfMass()), inDirection);
 	}
 
 	/// Transform this shape cast using inTransform. Multiply transform on the left left hand side.
-	ShapeCast					PostTransformed(Mat44Arg inTransform) const
+	ShapeCastType				PostTransformed(typename Mat::ArgType inTransform) const
 	{
 		Mat44 start = inTransform * mCenterOfMassStart;
 		Vec3 direction = inTransform.Multiply3x3(mDirection);
 		return { mShape, mScale, start, direction };
 	}
 
+	/// Translate this shape cast by inTranslation.
+	ShapeCastType				PostTranslated(typename Vec::ArgType inTranslation) const
+	{
+		return { mShape, mScale, mCenterOfMassStart.PostTranslated(inTranslation), mDirection };
+	}
+
+	/// Get point with fraction inFraction on ray from mCenterOfMassStart to mCenterOfMassStart + mDirection (0 = start of ray, 1 = end of ray)
+	inline Vec					GetPointOnRay(float inFraction) const
+	{
+		return mCenterOfMassStart.GetTranslation() + inFraction * mDirection;
+	}
+
 	const Shape *				mShape;								///< Shape that's being cast (cannot be mesh shape). Note that this structure does not assume ownership over the shape for performance reasons.
 	const Vec3					mScale;								///< Scale in local space of the shape being cast
-	const Mat44					mCenterOfMassStart;					///< Start position and orientation of the center of mass of the shape (construct using sFromWorldTransform if you have a world transform for your shape)
+	const Mat					mCenterOfMassStart;					///< Start position and orientation of the center of mass of the shape (construct using sFromWorldTransform if you have a world transform for your shape)
 	const Vec3					mDirection;							///< Direction and length of the cast (anything beyond this length will not be reported as a hit)
 	const AABox					mShapeWorldBounds;					///< Cached shape's world bounds, calculated in constructor
 };
 
+struct ShapeCast : public ShapeCastT<Vec3, Mat44, ShapeCast>
+{
+	using ShapeCastT<Vec3, Mat44, ShapeCast>::ShapeCastT;
+};
+
+struct RShapeCast : public ShapeCastT<RVec3, RMat44, RShapeCast>
+{
+	using ShapeCastT<RVec3, RMat44, RShapeCast>::ShapeCastT;
+
+	/// Convert from ShapeCast, converts single to double precision
+	explicit					RShapeCast(const ShapeCast &inCast) :
+		RShapeCast(inCast.mShape, inCast.mScale, RMat44(inCast.mCenterOfMassStart), inCast.mDirection, inCast.mShapeWorldBounds)
+	{
+	}
+
+	/// Convert to ShapeCast, which implies casting from double precision to single precision
+	explicit					operator ShapeCast() const
+	{
+		return ShapeCast(mShape, mScale, mCenterOfMassStart.ToMat44(), mDirection, mShapeWorldBounds);
+	}
+};
+
 /// Settings to be passed with a shape cast
 class ShapeCastSettings : public CollideSettingsBase
 {

Jolt/Physics/Collision/TransformedShape.cpp

@@ -13,12 +13,12 @@
 
 JPH_NAMESPACE_BEGIN
 
-bool TransformedShape::CastRay(const RayCast &inRay, RayCastResult &ioHit) const
+bool TransformedShape::CastRay(const RRayCast &inRay, RayCastResult &ioHit) const
 {
 	if (mShape != nullptr)
 	{
-		// Transform the ray to local space
-		RayCast ray = inRay.Transformed(GetInverseCenterOfMassTransform());
+		// Transform the ray to local space, note that this drops precision which is possible because we're in local space now
+		RayCast ray(inRay.Transformed(GetInverseCenterOfMassTransform()));
 
 		// Scale the ray
 		Vec3 inv_scale = GetShapeScale().Reciprocal();
@@ -39,7 +39,7 @@ bool TransformedShape::CastRay(const RayCast &inRay, RayCastResult &ioHit) const
 	return false;
 }
 
-void TransformedShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+void TransformedShape::CastRay(const RRayCast &inRay, const RayCastSettings &inRayCastSettings, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const
 {
 	if (mShape != nullptr)
 	{
@@ -47,8 +47,8 @@ void TransformedShape::CastRay(const RayCast &inRay, const RayCastSettings &inRa
 		ioCollector.SetContext(this);
 		inShapeFilter.mBodyID2 = mBodyID;
 
-		// Transform and scale the ray to local space
-		RayCast ray = inRay.Transformed(GetInverseCenterOfMassTransform());
+		// Transform the ray to local space, note that this drops precision which is possible because we're in local space now
+		RayCast ray(inRay.Transformed(GetInverseCenterOfMassTransform()));
 
 		// Scale the ray
 		Vec3 inv_scale = GetShapeScale().Reciprocal();
@@ -61,7 +61,7 @@ void TransformedShape::CastRay(const RayCast &inRay, const RayCastSettings &inRa
 	}
 }
 
-void TransformedShape::CollidePoint(Vec3Arg inPoint, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+void TransformedShape::CollidePoint(RVec3Arg inPoint, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
 {
 	if (mShape != nullptr)
 	{
@@ -70,7 +70,7 @@ void TransformedShape::CollidePoint(Vec3Arg inPoint, CollidePointCollector &ioCo
 		inShapeFilter.mBodyID2 = mBodyID;
 
 		// Transform and scale the point to local space
-		Vec3 point = (GetInverseCenterOfMassTransform() * inPoint) / GetShapeScale();
+		Vec3 point = Vec3(GetInverseCenterOfMassTransform() * inPoint) / GetShapeScale();
 
 		// Do point collide on the shape
 		SubShapeIDCreator sub_shape_id(mSubShapeIDCreator);
@@ -78,7 +78,7 @@ void TransformedShape::CollidePoint(Vec3Arg inPoint, CollidePointCollector &ioCo
 	}
 }
 
-void TransformedShape::CollideShape(const Shape *inShape, Vec3Arg inShapeScale, Mat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+void TransformedShape::CollideShape(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const
 {
 	if (mShape != nullptr)
 	{
@@ -87,11 +87,13 @@ void TransformedShape::CollideShape(const Shape *inShape, Vec3Arg inShapeScale,
 		inShapeFilter.mBodyID2 = mBodyID;
 
 		SubShapeIDCreator sub_shape_id1, sub_shape_id2(mSubShapeIDCreator);
-		CollisionDispatch::sCollideShapeVsShape(inShape, mShape, inShapeScale, GetShapeScale(), inCenterOfMassTransform, GetCenterOfMassTransform(), sub_shape_id1, sub_shape_id2, inCollideShapeSettings, ioCollector, inShapeFilter);
+		Mat44 transform1 = inCenterOfMassTransform.PostTranslated(-inBaseOffset).ToMat44();
+		Mat44 transform2 = GetCenterOfMassTransform().PostTranslated(-inBaseOffset).ToMat44();
+		CollisionDispatch::sCollideShapeVsShape(inShape, mShape, inShapeScale, GetShapeScale(), transform1, transform2, sub_shape_id1, sub_shape_id2, inCollideShapeSettings, ioCollector, inShapeFilter);
 	}
 }
 
-void TransformedShape::CastShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, CastShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const
+void TransformedShape::CastShape(const RShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const
 {
 	if (mShape != nullptr)
 	{
@@ -99,11 +101,14 @@ void TransformedShape::CastShape(const ShapeCast &inShapeCast, const ShapeCastSe
 		ioCollector.SetContext(this);
 		inShapeFilter.mBodyID2 = mBodyID;
 
-		// Get center of mass of object we're casting against
-		Mat44 center_of_mass_transform2 = GetCenterOfMassTransform();
+		// Get the shape cast relative to the base offset and convert it to floats
+		ShapeCast shape_cast(inShapeCast.PostTranslated(-inBaseOffset));
+
+		// Get center of mass of object we're casting against relative to the base offset and convert it to floats
+		Mat44 center_of_mass_transform2 = GetCenterOfMassTransform().PostTranslated(-inBaseOffset).ToMat44();
 
 		SubShapeIDCreator sub_shape_id1, sub_shape_id2(mSubShapeIDCreator);
-		CollisionDispatch::sCastShapeVsShapeWorldSpace(inShapeCast, inShapeCastSettings, mShape, GetShapeScale(), inShapeFilter, center_of_mass_transform2, sub_shape_id1, sub_shape_id2, ioCollector);
+		CollisionDispatch::sCastShapeVsShapeWorldSpace(shape_cast, inShapeCastSettings, mShape, GetShapeScale(), inShapeFilter, center_of_mass_transform2, sub_shape_id1, sub_shape_id2, ioCollector);
 	}
 }
 
@@ -111,17 +116,56 @@ void TransformedShape::CollectTransformedShapes(const AABox &inBox, TransformedS
 {
 	if (mShape != nullptr)
 	{
+		struct MyCollector : public TransformedShapeCollector
+		{
+										MyCollector(TransformedShapeCollector &ioCollector, RVec3 inShapePositionCOM) :
+				mCollector(ioCollector),
+				mShapePositionCOM(inShapePositionCOM)
+			{
+				UpdateEarlyOutFraction(ioCollector.GetEarlyOutFraction());
+				SetContext(ioCollector.GetContext());
+			}
+
+			virtual void				AddHit(const TransformedShape &inResult) override
+			{
+				// Apply the center of mass offset
+				TransformedShape ts = inResult;
+				ts.mShapePositionCOM += mShapePositionCOM;
+
+				// Pass hit on to child collector
+				mCollector.AddHit(ts);
+
+				mCollector.UpdateEarlyOutFraction(GetEarlyOutFraction());
+			}
+
+			TransformedShapeCollector &	mCollector;
+			RVec3						mShapePositionCOM;
+		};
+
 		// Set the context on the collector
 		ioCollector.SetContext(this);
 
-		mShape->CollectTransformedShapes(inBox, mShapePositionCOM, mShapeRotation, GetShapeScale(), mSubShapeIDCreator, ioCollector, inShapeFilter);
+		// Wrap the collector so we can add the center of mass precision, we do this to avoid losing precision because CollectTransformedShapes uses single precision floats
+		MyCollector collector(ioCollector, mShapePositionCOM);
+
+		// Take box to local space for the shape
+		AABox box = inBox;
+		box.Translate(-mShapePositionCOM);
+
+		mShape->CollectTransformedShapes(box, Vec3::sZero(), mShapeRotation, GetShapeScale(), mSubShapeIDCreator, collector, inShapeFilter);
 	}
 }
 
-void TransformedShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox) const
+void TransformedShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, RVec3Arg inBaseOffset) const
 {
 	if (mShape != nullptr)
-		mShape->GetTrianglesStart(ioContext, inBox, mShapePositionCOM, mShapeRotation, GetShapeScale());
+	{
+		// Take box to local space for the shape
+		AABox box = inBox;
+		box.Translate(-inBaseOffset);
+
+		mShape->GetTrianglesStart(ioContext, box, Vec3(mShapePositionCOM - inBaseOffset), mShapeRotation, GetShapeScale());
+	}
 }
 
 int TransformedShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const