@bloomengine/engine 0.4.0 → 0.4.2

package/native/third_party/JoltPhysics/Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h

@@ -1,283 +0,0 @@
-// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
-// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
-// SPDX-License-Identifier: MIT
-
-#pragma once
-
-#include <Jolt/Physics/Body/Body.h>
-#include <Jolt/Physics/StateRecorder.h>
-
-JPH_NAMESPACE_BEGIN
-
-/// Constrains rotation around all axis so that only translation is allowed
-///
-/// Based on: "Constraints Derivation for Rigid Body Simulation in 3D" - Daniel Chappuis, section 2.5.1
-///
-/// Constraint equation (eq 129):
-///
-/// \f[C = \begin{bmatrix}\Delta\theta_x, \Delta\theta_y, \Delta\theta_z\end{bmatrix}\f]
-///
-/// Jacobian (eq 131):
-///
-/// \f[J = \begin{bmatrix}0 & -E & 0 & E\end{bmatrix}\f]
-///
-/// Used terms (here and below, everything in world space):\n
-/// delta_theta_* = difference in rotation between initial rotation of bodies 1 and 2.\n
-/// x1, x2 = center of mass for the bodies.\n
-/// v = [v1, w1, v2, w2].\n
-/// v1, v2 = linear velocity of body 1 and 2.\n
-/// w1, w2 = angular velocity of body 1 and 2.\n
-/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n
-/// \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n
-/// b = velocity bias.\n
-/// \f$\beta\f$ = baumgarte constant.\n
-/// E = identity matrix.\n
-class RotationEulerConstraintPart
-{
-private:
-	/// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated
-	JPH_INLINE bool				ApplyVelocityStep(Body &ioBody1, Body &ioBody2, Vec3Arg inLambda) const
-	{
-		// Apply impulse if delta is not zero
-		if (inLambda != Vec3::sZero())
-		{
-			// Calculate velocity change due to constraint
-			//
-			// Impulse:
-			// P = J^T lambda
-			//
-			// Euler velocity integration:
-			// v' = v + M^-1 P
-			if (ioBody1.IsDynamic())
-				ioBody1.GetMotionProperties()->SubAngularVelocityStep(mInvI1.Multiply3x3(inLambda));
-			if (ioBody2.IsDynamic())
-				ioBody2.GetMotionProperties()->AddAngularVelocityStep(mInvI2.Multiply3x3(inLambda));
-			return true;
-		}
-
-		return false;
-	}
-
-public:
-	/// Return inverse of initial rotation from body 1 to body 2 in body 1 space
-	static Quat					sGetInvInitialOrientation(const Body &inBody1, const Body &inBody2)
-	{
-		// q20 = q10 r0
-		// <=> r0 = q10^-1 q20
-		// <=> r0^-1 = q20^-1 q10
-		//
-		// where:
-		//
-		// q20 = initial orientation of body 2
-		// q10 = initial orientation of body 1
-		// r0 = initial rotation from body 1 to body 2
-		return inBody2.GetRotation().Conjugated() * inBody1.GetRotation();
-	}
-
-	/// @brief Return inverse of initial rotation from body 1 to body 2 in body 1 space
-	/// @param inAxisX1 Reference axis X for body 1
-	/// @param inAxisY1 Reference axis Y for body 1
-	/// @param inAxisX2 Reference axis X for body 2
-	/// @param inAxisY2 Reference axis Y for body 2
-	static Quat					sGetInvInitialOrientationXY(Vec3Arg inAxisX1, Vec3Arg inAxisY1, Vec3Arg inAxisX2, Vec3Arg inAxisY2)
-	{
-		// Store inverse of initial rotation from body 1 to body 2 in body 1 space:
-		//
-		// q20 = q10 r0
-		// <=> r0 = q10^-1 q20
-		// <=> r0^-1 = q20^-1 q10
-		//
-		// where:
-		//
-		// q10, q20 = world space initial orientation of body 1 and 2
-		// r0 = initial rotation from body 1 to body 2 in local space of body 1
-		//
-		// We can also write this in terms of the constraint matrices:
-		//
-		// q20 c2 = q10 c1
-		// <=> q20 = q10 c1 c2^-1
-		// => r0 = c1 c2^-1
-		// <=> r0^-1 = c2 c1^-1
-		//
-		// where:
-		//
-		// c1, c2 = matrix that takes us from body 1 and 2 COM to constraint space 1 and 2
-		if (inAxisX1 == inAxisX2 && inAxisY1 == inAxisY2)
-		{
-			// Axis are the same -> identity transform
-			return Quat::sIdentity();
-		}
-		else
-		{
-			Mat44 constraint1(Vec4(inAxisX1, 0), Vec4(inAxisY1, 0), Vec4(inAxisX1.Cross(inAxisY1), 0), Vec4(0, 0, 0, 1));
-			Mat44 constraint2(Vec4(inAxisX2, 0), Vec4(inAxisY2, 0), Vec4(inAxisX2.Cross(inAxisY2), 0), Vec4(0, 0, 0, 1));
-			return constraint2.GetQuaternion() * constraint1.GetQuaternion().Conjugated();
-		}
-	}
-
-	/// @brief Return inverse of initial rotation from body 1 to body 2 in body 1 space
-	/// @param inAxisX1 Reference axis X for body 1
-	/// @param inAxisZ1 Reference axis Z for body 1
-	/// @param inAxisX2 Reference axis X for body 2
-	/// @param inAxisZ2 Reference axis Z for body 2
-	static Quat					sGetInvInitialOrientationXZ(Vec3Arg inAxisX1, Vec3Arg inAxisZ1, Vec3Arg inAxisX2, Vec3Arg inAxisZ2)
-	{
-		// See comment at sGetInvInitialOrientationXY
-		if (inAxisX1 == inAxisX2 && inAxisZ1 == inAxisZ2)
-		{
-			return Quat::sIdentity();
-		}
-		else
-		{
-			Mat44 constraint1(Vec4(inAxisX1, 0), Vec4(inAxisZ1.Cross(inAxisX1), 0), Vec4(inAxisZ1, 0), Vec4(0, 0, 0, 1));
-			Mat44 constraint2(Vec4(inAxisX2, 0), Vec4(inAxisZ2.Cross(inAxisX2), 0), Vec4(inAxisZ2, 0), Vec4(0, 0, 0, 1));
-			return constraint2.GetQuaternion() * constraint1.GetQuaternion().Conjugated();
-		}
-	}
-
-	/// Calculate properties used during the functions below
-	inline void					CalculateConstraintProperties(const Body &inBody1, Mat44Arg inRotation1, const Body &inBody2, Mat44Arg inRotation2)
-	{
-		// Calculate properties used during constraint solving
-		mInvI1 = inBody1.IsDynamic()? inBody1.GetMotionProperties()->GetInverseInertiaForRotation(inRotation1) : Mat44::sZero();
-		mInvI2 = inBody2.IsDynamic()? inBody2.GetMotionProperties()->GetInverseInertiaForRotation(inRotation2) : Mat44::sZero();
-
-		// Calculate effective mass: K^-1 = (J M^-1 J^T)^-1
-		Mat44 inertia_sum = mInvI1 + mInvI2;
-		if (!mEffectiveMass.SetInversed3x3(inertia_sum))
-		{
-			// If a column is zero, the axis is locked and we set the column to identity.
-			// This does not matter because any impulse will always be multiplied with mInvI1 or mInvI2 which will result in zero for the locked coordinate.
-			Vec4 zero = Vec4::sZero();
-			if (inertia_sum.GetColumn4(0) == zero)
-				inertia_sum.SetColumn4(0, Vec4(1, 0, 0, 0));
-			if (inertia_sum.GetColumn4(1) == zero)
-				inertia_sum.SetColumn4(1, Vec4(0, 1, 0, 0));
-			if (inertia_sum.GetColumn4(2) == zero)
-				inertia_sum.SetColumn4(2, Vec4(0, 0, 1, 0));
-			if (!mEffectiveMass.SetInversed3x3(inertia_sum))
-				Deactivate();
-		}
-	}
-
-	/// Deactivate this constraint
-	inline void					Deactivate()
-	{
-		mEffectiveMass = Mat44::sZero();
-		mTotalLambda = Vec3::sZero();
-	}
-
-	/// Check if constraint is active
-	inline bool					IsActive() const
-	{
-		return mEffectiveMass(3, 3) != 0.0f;
-	}
-
-	/// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses
-	inline void					WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio)
-	{
-		mTotalLambda *= inWarmStartImpulseRatio;
-		ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda);
-	}
-
-	/// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation.
-	inline bool					SolveVelocityConstraint(Body &ioBody1, Body &ioBody2)
-	{
-		// Calculate lagrange multiplier:
-		//
-		// lambda = -K^-1 (J v + b)
-		Vec3 lambda = mEffectiveMass.Multiply3x3(ioBody1.GetAngularVelocity() - ioBody2.GetAngularVelocity());
-		mTotalLambda += lambda;
-		return ApplyVelocityStep(ioBody1, ioBody2, lambda);
-	}
-
-	/// Iteratively update the position constraint. Makes sure C(...) = 0.
-	inline bool					SolvePositionConstraint(Body &ioBody1, Body &ioBody2, QuatArg inInvInitialOrientation, float inBaumgarte) const
-	{
-		// Calculate difference in rotation
-		//
-		// The rotation should be:
-		//
-		// q2 = q1 r0
-		//
-		// But because of drift the actual rotation is
-		//
-		// q2 = diff q1 r0
-		// <=> diff = q2 r0^-1 q1^-1
-		//
-		// Where:
-		// q1 = current rotation of body 1
-		// q2 = current rotation of body 2
-		// diff = error that needs to be reduced to zero
-		Quat diff = ioBody2.GetRotation() * inInvInitialOrientation * ioBody1.GetRotation().Conjugated();
-
-		// A quaternion can be seen as:
-		//
-		// q = [sin(theta / 2) * v, cos(theta/2)]
-		//
-		// Where:
-		// v = rotation vector
-		// theta = rotation angle
-		//
-		// If we assume theta is small (error is small) then sin(x) = x so an approximation of the error angles is:
-		Vec3 error = 2.0f * diff.EnsureWPositive().GetXYZ();
-		if (error != Vec3::sZero())
-		{
-			// Calculate lagrange multiplier (lambda) for Baumgarte stabilization:
-			//
-			// lambda = -K^-1 * beta / dt * C
-			//
-			// We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out
-			Vec3 lambda = -inBaumgarte * mEffectiveMass * error;
-
-			// Directly integrate velocity change for one time step
-			//
-			// Euler velocity integration:
-			// dv = M^-1 P
-			//
-			// Impulse:
-			// P = J^T lambda
-			//
-			// Euler position integration:
-			// x' = x + dv * dt
-			//
-			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
-			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
-			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
-			// integrate + a position integrate and then discard the velocity change.
-			if (ioBody1.IsDynamic())
-				ioBody1.SubRotationStep(mInvI1.Multiply3x3(lambda));
-			if (ioBody2.IsDynamic())
-				ioBody2.AddRotationStep(mInvI2.Multiply3x3(lambda));
-			return true;
-		}
-
-		return false;
-	}
-
-	/// Return lagrange multiplier
-	Vec3						GetTotalLambda() const
-	{
-		return mTotalLambda;
-	}
-
-	/// Save state of this constraint part
-	void						SaveState(StateRecorder &inStream) const
-	{
-		inStream.Write(mTotalLambda);
-	}
-
-	/// Restore state of this constraint part
-	void						RestoreState(StateRecorder &inStream)
-	{
-		inStream.Read(mTotalLambda);
-	}
-
-private:
-	Mat44						mInvI1;
-	Mat44						mInvI2;
-	Mat44						mEffectiveMass;
-	Vec3						mTotalLambda { Vec3::sZero() };
-};
-
-JPH_NAMESPACE_END

package/native/third_party/JoltPhysics/Jolt/Physics/Constraints/ConstraintPart/RotationQuatConstraintPart.h

@@ -1,246 +0,0 @@
-// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
-// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
-// SPDX-License-Identifier: MIT
-
-#pragma once
-
-#include <Jolt/Physics/Body/Body.h>
-#include <Jolt/Physics/StateRecorder.h>
-
-JPH_NAMESPACE_BEGIN
-
-/// Quaternion based constraint that constrains rotation around all axis so that only translation is allowed.
-///
-/// NOTE: This constraint part is more expensive than the RotationEulerConstraintPart and slightly more correct since
-/// RotationEulerConstraintPart::SolvePositionConstraint contains an approximation. In practice the difference
-/// is small, so the RotationEulerConstraintPart is probably the better choice.
-///
-/// Rotation is fixed between bodies like this:
-///
-/// q2 = q1 r0
-///
-/// Where:
-/// q1, q2 = world space quaternions representing rotation of body 1 and 2.
-/// r0 = initial rotation between bodies in local space of body 1, this can be calculated by:
-///
-/// q20 = q10 r0
-/// <=> r0 = q10^* q20
-///
-/// Where:
-/// q10, q20 = initial world space rotations of body 1 and 2.
-/// q10^* = conjugate of quaternion q10 (which is the same as the inverse for a unit quaternion)
-///
-/// We exclusively use the conjugate below:
-///
-/// r0^* = q20^* q10
-///
-/// The error in the rotation is (in local space of body 1):
-///
-/// q2 = q1 error r0
-/// <=> error = q1^* q2 r0^*
-///
-/// The imaginary part of the quaternion represents the rotation axis * sin(angle / 2). The real part of the quaternion
-/// does not add any additional information (we know the quaternion in normalized) and we're removing 3 degrees of freedom
-/// so we want 3 parameters. Therefore we define the constraint equation like:
-///
-/// C = A q1^* q2 r0^* = 0
-///
-/// Where (if you write a quaternion as [real-part, i-part, j-part, k-part]):
-///
-///		    [0, 1, 0, 0]
-///		A = [0, 0, 1, 0]
-///		    [0, 0, 0, 1]
-///
-/// or in our case since we store a quaternion like [i-part, j-part, k-part, real-part]:
-///
-///		    [1, 0, 0, 0]
-///		A = [0, 1, 0, 0]
-///		    [0, 0, 1, 0]
-///
-/// Time derivative:
-///
-/// d/dt C = A (q1^* d/dt(q2) + d/dt(q1^*) q2) r0^*
-/// = A (q1^* (1/2 W2 q2) + (1/2 W1 q1)^* q2) r0^*
-/// = 1/2 A (q1^* W2 q2 + q1^* W1^* q2) r0^*
-/// = 1/2 A (q1^* W2 q2 - q1^* W1 * q2) r0^*
-/// = 1/2 A ML(q1^*) MR(q2 r0^*) (W2 - W1)
-/// = 1/2 A ML(q1^*) MR(q2 r0^*) A^T (w2 - w1)
-///
-/// Where:
-/// W1 = [0, w1], W2 = [0, w2] (converting angular velocity to imaginary part of quaternion).
-/// w1, w2 = angular velocity of body 1 and 2.
-/// d/dt(q) = 1/2 W q (time derivative of a quaternion).
-/// W^* = -W (conjugate negates angular velocity as quaternion).
-/// ML(q): 4x4 matrix so that q * p = ML(q) * p, where q and p are quaternions.
-/// MR(p): 4x4 matrix so that q * p = MR(p) * q, where q and p are quaternions.
-/// A^T: Transpose of A.
-///
-/// Jacobian:
-///
-/// J = [0, -1/2 A ML(q1^*) MR(q2 r0^*) A^T, 0, 1/2 A ML(q1^*) MR(q2 r0^*) A^T]
-/// = [0, -JP, 0, JP]
-///
-/// Suggested reading:
-/// - 3D Constraint Derivations for Impulse Solvers - Marijn Tamis
-/// - Game Physics Pearls - Section 9 - Quaternion Based Constraints - Claude Lacoursiere
-class RotationQuatConstraintPart
-{
-private:
-	/// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated
-	JPH_INLINE bool				ApplyVelocityStep(Body &ioBody1, Body &ioBody2, Vec3Arg inLambda) const
-	{
-		// Apply impulse if delta is not zero
-		if (inLambda != Vec3::sZero())
-		{
-			// Calculate velocity change due to constraint
-			//
-			// Impulse:
-			// P = J^T lambda
-			//
-			// Euler velocity integration:
-			// v' = v + M^-1 P
-			if (ioBody1.IsDynamic())
-				ioBody1.GetMotionProperties()->SubAngularVelocityStep(mInvI1_JPT.Multiply3x3(inLambda));
-			if (ioBody2.IsDynamic())
-				ioBody2.GetMotionProperties()->AddAngularVelocityStep(mInvI2_JPT.Multiply3x3(inLambda));
-			return true;
-		}
-
-		return false;
-	}
-
-public:
-	/// Return inverse of initial rotation from body 1 to body 2 in body 1 space
-	static Quat					sGetInvInitialOrientation(const Body &inBody1, const Body &inBody2)
-	{
-		// q20 = q10 r0
-		// <=> r0 = q10^-1 q20
-		// <=> r0^-1 = q20^-1 q10
-		//
-		// where:
-		//
-		// q20 = initial orientation of body 2
-		// q10 = initial orientation of body 1
-		// r0 = initial rotation from body 1 to body 2
-		return inBody2.GetRotation().Conjugated() * inBody1.GetRotation();
-	}
-
-	/// Calculate properties used during the functions below
-	inline void					CalculateConstraintProperties(const Body &inBody1, Mat44Arg inRotation1, const Body &inBody2, Mat44Arg inRotation2, QuatArg inInvInitialOrientation)
-	{
-		// Calculate: JP = 1/2 A ML(q1^*) MR(q2 r0^*) A^T
-		Mat44 jp = (Mat44::sQuatLeftMultiply(0.5f * inBody1.GetRotation().Conjugated()) * Mat44::sQuatRightMultiply(inBody2.GetRotation() * inInvInitialOrientation)).GetRotationSafe();
-
-		// Calculate properties used during constraint solving
-		Mat44 inv_i1 = inBody1.IsDynamic()? inBody1.GetMotionProperties()->GetInverseInertiaForRotation(inRotation1) : Mat44::sZero();
-		Mat44 inv_i2 = inBody2.IsDynamic()? inBody2.GetMotionProperties()->GetInverseInertiaForRotation(inRotation2) : Mat44::sZero();
-		mInvI1_JPT = inv_i1.Multiply3x3RightTransposed(jp);
-		mInvI2_JPT = inv_i2.Multiply3x3RightTransposed(jp);
-
-		// Calculate effective mass: K^-1 = (J M^-1 J^T)^-1
-		// = (JP * I1^-1 * JP^T + JP * I2^-1 * JP^T)^-1
-		// = (JP * (I1^-1 + I2^-1) * JP^T)^-1
-		if (!mEffectiveMass.SetInversed3x3(jp.Multiply3x3(inv_i1 + inv_i2).Multiply3x3RightTransposed(jp)))
-			Deactivate();
-		else
-			mEffectiveMass_JP = mEffectiveMass.Multiply3x3(jp);
-	}
-
-	/// Deactivate this constraint
-	inline void					Deactivate()
-	{
-		mEffectiveMass = Mat44::sZero();
-		mEffectiveMass_JP = Mat44::sZero();
-		mTotalLambda = Vec3::sZero();
-	}
-
-	/// Check if constraint is active
-	inline bool					IsActive() const
-	{
-		return mEffectiveMass(3, 3) != 0.0f;
-	}
-
-	/// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses
-	inline void					WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio)
-	{
-		mTotalLambda *= inWarmStartImpulseRatio;
-		ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda);
-	}
-
-	/// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation.
-	inline bool					SolveVelocityConstraint(Body &ioBody1, Body &ioBody2)
-	{
-		// Calculate lagrange multiplier:
-		//
-		// lambda = -K^-1 (J v + b)
-		Vec3 lambda = mEffectiveMass_JP.Multiply3x3(ioBody1.GetAngularVelocity() - ioBody2.GetAngularVelocity());
-		mTotalLambda += lambda;
-		return ApplyVelocityStep(ioBody1, ioBody2, lambda);
-	}
-
-	/// Iteratively update the position constraint. Makes sure C(...) = 0.
-	inline bool					SolvePositionConstraint(Body &ioBody1, Body &ioBody2, QuatArg inInvInitialOrientation, float inBaumgarte) const
-	{
-		// Calculate constraint equation
-		Vec3 c = (ioBody1.GetRotation().Conjugated() * ioBody2.GetRotation() * inInvInitialOrientation).GetXYZ();
-		if (c != Vec3::sZero())
-		{
-			// Calculate lagrange multiplier (lambda) for Baumgarte stabilization:
-			//
-			// lambda = -K^-1 * beta / dt * C
-			//
-			// We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out
-			Vec3 lambda = -inBaumgarte * mEffectiveMass * c;
-
-			// Directly integrate velocity change for one time step
-			//
-			// Euler velocity integration:
-			// dv = M^-1 P
-			//
-			// Impulse:
-			// P = J^T lambda
-			//
-			// Euler position integration:
-			// x' = x + dv * dt
-			//
-			// Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and
-			// Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte
-			// stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity
-			// integrate + a position integrate and then discard the velocity change.
-			if (ioBody1.IsDynamic())
-				ioBody1.SubRotationStep(mInvI1_JPT.Multiply3x3(lambda));
-			if (ioBody2.IsDynamic())
-				ioBody2.AddRotationStep(mInvI2_JPT.Multiply3x3(lambda));
-			return true;
-		}
-
-		return false;
-	}
-
-	/// Return lagrange multiplier
-	Vec3						GetTotalLambda() const
-	{
-		return mTotalLambda;
-	}
-
-	/// Save state of this constraint part
-	void						SaveState(StateRecorder &inStream) const
-	{
-		inStream.Write(mTotalLambda);
-	}
-
-	/// Restore state of this constraint part
-	void						RestoreState(StateRecorder &inStream)
-	{
-		inStream.Read(mTotalLambda);
-	}
-
-private:
-	Mat44						mInvI1_JPT;
-	Mat44						mInvI2_JPT;
-	Mat44						mEffectiveMass;
-	Mat44						mEffectiveMass_JP;
-	Vec3						mTotalLambda { Vec3::sZero() };
-};
-
-JPH_NAMESPACE_END