Added functionality to compress a unit vector/quat to a uint32 (#1708)

Added Vec4::sClamp, GetLowestComponentIndex and GetHighestComponentIndex
Added assignment operator for Float4

Jolt/Math/Float4.h

@@ -15,6 +15,7 @@ public:
 				Float4() = default; ///< Intentionally not initialized for performance reasons
 				Float4(const Float4 &inRHS) = default;
 				Float4(float inX, float inY, float inZ, float inW) : x(inX), y(inY), z(inZ), w(inW) { }
+	Float4 &	operator = (const Float4 &inRHS) = default;
 
 	float		operator [] (int inCoordinate) const
 	{

Jolt/Math/Quat.h

@@ -248,6 +248,12 @@ public:
 	/// Store as 4 floats
 	JPH_INLINE void				StoreFloat4(Float4 *outV) const;
 
+	/// Compress a unit quaternion to a 32 bit value, precision is around 0.5 degree
+	JPH_INLINE uint32			CompressUnitQuat() const										{ return mValue.CompressUnitVector(); }
+
+	/// Decompress a unit quaternion from a 32 bit value
+	JPH_INLINE static Quat		sDecompressUnitQuat(uint32 inValue)								{ return Quat(Vec4::sDecompressUnitVector(inValue)); }
+
 	/// To String
 	friend ostream &			operator << (ostream &inStream, QuatArg inQ)					{ inStream << inQ.mValue; return inStream; }
 

Jolt/Math/Vec3.h

@@ -275,6 +275,12 @@ public:
 	template <int X, int Y, int Z>
 	JPH_INLINE Vec3				FlipSign() const;
 
+	/// Compress a unit vector to a 32 bit value, precision is around 10^-4
+	JPH_INLINE uint32			CompressUnitVector() const;
+
+	/// Decompress a unit vector from a 32 bit value
+	JPH_INLINE static Vec3		sDecompressUnitVector(uint32 inValue);
+
 	/// To String
 	friend ostream &			operator << (ostream &inStream, Vec3Arg inV)
 	{

Jolt/Math/Vec3.inl

@@ -866,4 +866,73 @@ JPH_INLINE Vec3 Vec3::FlipSign() const
 	return Vec3::sXor(*this, Vec3(X > 0? 0.0f : -0.0f, Y > 0? 0.0f : -0.0f, Z > 0? 0.0f : -0.0f));
 }
 
+uint32 Vec3::CompressUnitVector() const
+{
+	constexpr float cOneOverSqrt2 = 0.70710678f;
+	constexpr uint cNumBits = 14;
+	constexpr uint cMask = (1 << cNumBits) - 1;
+
+	// Store sign bit
+	Vec3 v = *this;
+	uint32 max_element = v.Abs().GetHighestComponentIndex();
+	uint32 value = 0;
+	if (v[max_element] < 0.0f)
+	{
+		value = 0x80000000u;
+		v = -v;
+	}
+
+	// Store highest component
+	value |= max_element << 29;
+
+	// Store the other two components in a compressed format
+	UVec4 compressed = Vec3::sClamp((v + Vec3::sReplicate(cOneOverSqrt2)) * (float(cMask) / (2.0f * cOneOverSqrt2)) + Vec3::sReplicate(0.5f), Vec3::sZero(), Vec3::sReplicate(cMask)).ToInt();
+	switch (max_element)
+	{
+	case 0:
+		compressed = compressed.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_UNUSED, SWIZZLE_UNUSED>();
+		break;
+
+	case 1:
+		compressed = compressed.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_UNUSED, SWIZZLE_UNUSED>();
+		break;
+	}
+
+	value |= compressed.GetX();
+	value |= compressed.GetY() << cNumBits;
+	return value;
+}
+
+Vec3 Vec3::sDecompressUnitVector(uint32 inValue)
+{
+	constexpr float cOneOverSqrt2 = 0.70710678f;
+	constexpr uint cNumBits = 14;
+	constexpr uint cMask = (1u << cNumBits) - 1;
+
+	// Restore two components
+	Vec3 v = Vec3(UVec4(inValue & cMask, (inValue >> cNumBits) & cMask, 0, 0).ToFloat()) * (2.0f * cOneOverSqrt2 / float(cMask)) - Vec3(cOneOverSqrt2, cOneOverSqrt2, 0.0f);
+	JPH_ASSERT(v.GetZ() == 0.0f);
+
+	// Restore the highest component
+	v.SetZ(sqrt(max(1.0f - v.LengthSq(), 0.0f)));
+
+	// Extract sign
+	if ((inValue & 0x80000000u) != 0)
+		v = -v;
+
+	// Swizzle the components in place
+	switch ((inValue >> 29) & 3)
+	{
+	case 0:
+		v = v.Swizzle<SWIZZLE_Z, SWIZZLE_X, SWIZZLE_Y>();
+		break;
+
+	case 1:
+		v = v.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_Y>();
+		break;
+	}
+
+	return v;
+}
+
 JPH_NAMESPACE_END

Jolt/Math/Vec4.h

@@ -63,6 +63,9 @@ public:
 	/// Return the maximum of each of the components
 	static JPH_INLINE Vec4		sMax(Vec4Arg inV1, Vec4Arg inV2);
 
+	/// Clamp a vector between min and max (component wise)
+	static JPH_INLINE Vec4		sClamp(Vec4Arg inV, Vec4Arg inMin, Vec4Arg inMax);
+
 	/// Equals (component wise)
 	static JPH_INLINE UVec4		sEquals(Vec4Arg inV1, Vec4Arg inV2);
 
@@ -215,6 +218,12 @@ public:
 	/// Replicate the W component to all components
 	JPH_INLINE Vec3				SplatW3() const;
 
+	/// Get index of component with lowest value
+	JPH_INLINE int				GetLowestComponentIndex() const;
+
+	/// Get index of component with highest value
+	JPH_INLINE int				GetHighestComponentIndex() const;
+
 	/// Return the absolute value of each of the components
 	JPH_INLINE Vec4				Abs() const;
 
@@ -284,6 +293,12 @@ public:
 	/// Calculate the arc tangent of y / x using the signs of the arguments to determine the correct quadrant (returns value in the range [-PI, PI])
 	inline static Vec4			sATan2(Vec4Arg inY, Vec4Arg inX);
 
+	/// Compress a unit vector to a 32 bit value, precision is around 0.5 * 10^-3
+	JPH_INLINE uint32			CompressUnitVector() const;
+
+	/// Decompress a unit vector from a 32 bit value
+	JPH_INLINE static Vec4		sDecompressUnitVector(uint32 inValue);
+
 	/// To String
 	friend ostream &			operator << (ostream &inStream, Vec4Arg inV)
 	{

Jolt/Math/Vec4.inl

@@ -168,6 +168,11 @@ Vec4 Vec4::sMax(Vec4Arg inV1, Vec4Arg inV2)
 #endif
 }
 
+Vec4 Vec4::sClamp(Vec4Arg inV, Vec4Arg inMin, Vec4Arg inMax)
+{
+	return sMax(sMin(inV, inMax), inMin);
+}
+
 UVec4 Vec4::sEquals(Vec4Arg inV1, Vec4Arg inV2)
 {
 #if defined(JPH_USE_SSE)
@@ -653,6 +658,26 @@ Vec3 Vec4::SplatW3() const
 #endif
 }
 
+int Vec4::GetLowestComponentIndex() const
+{
+	// Get the minimum value in all 4 components
+	Vec4 value = Vec4::sMin(*this, Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z>());
+	value = Vec4::sMin(value, value.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_X, SWIZZLE_Y>());
+
+	// Compare with the original vector to find which component is equal to the minimum value
+	return CountTrailingZeros(Vec4::sEquals(*this, value).GetTrues());
+}
+
+int Vec4::GetHighestComponentIndex() const
+{
+	// Get the maximum value in all 4 components
+	Vec4 value = Vec4::sMax(*this, Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z>());
+	value = Vec4::sMax(value, value.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_X, SWIZZLE_Y>());
+
+	// Compare with the original vector to find which component is equal to the maximum value
+	return CountTrailingZeros(Vec4::sEquals(*this, value).GetTrues());
+}
+
 Vec4 Vec4::Abs() const
 {
 #if defined(JPH_USE_AVX512)
@@ -1042,4 +1067,82 @@ Vec4 Vec4::sATan2(Vec4Arg inY, Vec4Arg inX)
 	return atan;
 }
 
+uint32 Vec4::CompressUnitVector() const
+{
+	constexpr float cOneOverSqrt2 = 0.70710678f;
+	constexpr uint cNumBits = 9;
+	constexpr uint cMask = (1 << cNumBits) - 1;
+
+	// Store sign bit
+	Vec4 v = *this;
+	uint32 max_element = v.Abs().GetHighestComponentIndex();
+	uint32 value = 0;
+	if (v[max_element] < 0.0f)
+	{
+		value = 0x80000000u;
+		v = -v;
+	}
+
+	// Store highest component
+	value |= max_element << 29;
+
+	// Store the other three components in a compressed format
+	UVec4 compressed = Vec4::sClamp((v + Vec4::sReplicate(cOneOverSqrt2)) * (float(cMask) / (2.0f * cOneOverSqrt2)) + Vec4::sReplicate(0.5f), Vec4::sZero(), Vec4::sReplicate(cMask)).ToInt();
+	switch (max_element)
+	{
+	case 0:
+		compressed = compressed.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_UNUSED>();
+		break;
+
+	case 1:
+		compressed = compressed.Swizzle<SWIZZLE_X, SWIZZLE_Z, SWIZZLE_W, SWIZZLE_UNUSED>();
+		break;
+
+	case 2:
+		compressed = compressed.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_W, SWIZZLE_UNUSED>();
+		break;
+	}
+
+	value |= compressed.GetX();
+	value |= compressed.GetY() << cNumBits;
+	value |= compressed.GetZ() << 2 * cNumBits;
+	return value;
+}
+
+Vec4 Vec4::sDecompressUnitVector(uint32 inValue)
+{
+	constexpr float cOneOverSqrt2 = 0.70710678f;
+	constexpr uint cNumBits = 9;
+	constexpr uint cMask = (1u << cNumBits) - 1;
+
+	// Restore three components
+	Vec4 v = Vec4(UVec4(inValue & cMask, (inValue >> cNumBits) & cMask, (inValue >> (2 * cNumBits)) & cMask, 0).ToFloat()) * (2.0f * cOneOverSqrt2 / float(cMask)) - Vec4(cOneOverSqrt2, cOneOverSqrt2, cOneOverSqrt2, 0.0f);
+	JPH_ASSERT(v.GetW() == 0.0f);
+
+	// Restore the highest component
+	v.SetW(sqrt(max(1.0f - v.LengthSq(), 0.0f)));
+
+	// Extract sign
+	if ((inValue & 0x80000000u) != 0)
+		v = -v;
+
+	// Swizzle the components in place
+	switch ((inValue >> 29) & 3)
+	{
+	case 0:
+		v = v.Swizzle<SWIZZLE_W, SWIZZLE_X, SWIZZLE_Y, SWIZZLE_Z>();
+		break;
+
+	case 1:
+		v = v.Swizzle<SWIZZLE_X, SWIZZLE_W, SWIZZLE_Y, SWIZZLE_Z>();
+		break;
+
+	case 2:
+		v = v.Swizzle<SWIZZLE_X, SWIZZLE_Y, SWIZZLE_W, SWIZZLE_Z>();
+		break;
+	}
+
+	return v;
+}
+
 JPH_NAMESPACE_END

UnitTests/Math/QuatTests.cpp

@@ -503,4 +503,19 @@ TEST_SUITE("QuatTests")
 			CHECK_APPROX_EQUAL(r1, r2);
 		}
 	}
+
+	TEST_CASE("TestQuatCompressUnitQuat")
+	{
+		UnitTestRandom random;
+		for (int i = 0; i < 1000; ++i)
+		{
+			Quat quat = Quat::sRandom(random);
+			uint32 compressed = quat.CompressUnitQuat();
+			Quat decompressed = Quat::sDecompressUnitQuat(compressed);
+			Vec3 axis;
+			float angle;
+			(quat * decompressed.Conjugated()).GetAxisAngle(axis, angle);
+			CHECK(abs(angle) < 0.009f);
+		}
+	}
 }

UnitTests/Math/Vec3Tests.cpp

@@ -384,4 +384,17 @@ TEST_SUITE("Vec3Tests")
 		Vec3 v(1, 2, 3);
 		CHECK(ConvertToString(v) == "1, 2, 3");
 	}
+
+	TEST_CASE("TestVec3CompressUnitVector")
+	{
+		UnitTestRandom random;
+		for (int i = 0; i < 1000; ++i)
+		{
+			Vec3 v = Vec3::sRandom(random);
+			uint32 compressed = v.CompressUnitVector();
+			Vec3 decompressed = Vec3::sDecompressUnitVector(compressed);
+			float diff = (decompressed - v).Length();
+			CHECK(diff < 1.0e-4f);
+		}
+	}
 }

UnitTests/Math/Vec4Tests.cpp

@@ -90,6 +90,8 @@ TEST_SUITE("Vec4Tests")
 	{
 		Vec4 v1(1, 6, 3, 8);
 		Vec4 v2(5, 2, 7, 4);
+		Vec4 v3(5, 7, 2, 4);
+		Vec4 v4(7, 5, 4, 2);
 
 		CHECK(Vec4::sMin(v1, v2) == Vec4(1, 2, 3, 4));
 		CHECK(Vec4::sMax(v1, v2) == Vec4(5, 6, 7, 8));
@@ -98,6 +100,24 @@ TEST_SUITE("Vec4Tests")
 		CHECK(v1.ReduceMax() == 8);
 		CHECK(v2.ReduceMin() == 2);
 		CHECK(v2.ReduceMax() == 7);
+
+		CHECK(v1.GetLowestComponentIndex() == 0);
+		CHECK(v1.GetHighestComponentIndex() == 3);
+		CHECK(v2.GetLowestComponentIndex() == 1);
+		CHECK(v2.GetHighestComponentIndex() == 2);
+		CHECK(v3.GetLowestComponentIndex() == 2);
+		CHECK(v3.GetHighestComponentIndex() == 1);
+		CHECK(v4.GetLowestComponentIndex() == 3);
+		CHECK(v4.GetHighestComponentIndex() == 0);
+	}
+
+	TEST_CASE("TestVec4Clamp")
+	{
+		Vec4 v1(1, 2, 3, 4);
+		Vec4 v2(5, 6, 7, 8);
+		Vec4 v(-1, 3, 9, -11);
+
+		CHECK(Vec4::sClamp(v, v1, v2) == Vec4(1, 3, 7, 4));
 	}
 
 	TEST_CASE("TestVec4Comparisons")
@@ -748,4 +768,18 @@ TEST_SUITE("Vec4Tests")
 		Vec4 v(1, 2, 3, 4);
 		CHECK(ConvertToString(v) == "1, 2, 3, 4");
 	}
+
+	TEST_CASE("TestVec4CompressUnitVector")
+	{
+		UnitTestRandom random;
+		for (int i = 0; i < 1000; ++i)
+		{
+			std::uniform_real_distribution<float> scale(-1.0f, 1.0f);
+			Vec4 v = Vec4(scale(random), scale(random), scale(random), scale(random)).Normalized();
+			uint32 compressed = v.CompressUnitVector();
+			Vec4 decompressed = Vec4::sDecompressUnitVector(compressed);
+			float diff = (decompressed - v).Length();
+			CHECK(diff < 5.0e-3f);
+		}
+	}
 }