initial commit, 4.5 stable

thirdparty/jolt_physics/Jolt/Physics/Body/Body.inl

@@ -0,0 +1,197 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+JPH_NAMESPACE_BEGIN
+
+RMat44 Body::GetWorldTransform() const
+{
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read));
+
+	return RMat44::sRotationTranslation(mRotation, mPosition).PreTranslated(-mShape->GetCenterOfMass());
+}
+
+RMat44 Body::GetCenterOfMassTransform() const
+{
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read));
+
+	return RMat44::sRotationTranslation(mRotation, mPosition);
+}
+
+RMat44 Body::GetInverseCenterOfMassTransform() const
+{
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read));
+
+	return RMat44::sInverseRotationTranslation(mRotation, mPosition);
+}
+
+inline bool Body::sFindCollidingPairsCanCollide(const Body &inBody1, const Body &inBody2)
+{
+	// First body should never be a soft body
+	JPH_ASSERT(!inBody1.IsSoftBody());
+
+	// One of these conditions must be true
+	// - We always allow detecting collisions between kinematic and non-dynamic bodies
+	// - One of the bodies must be dynamic to collide
+	// - A kinematic object can collide with a sensor
+	if (!inBody1.GetCollideKinematicVsNonDynamic()
+		&& !inBody2.GetCollideKinematicVsNonDynamic()
+		&& (!inBody1.IsDynamic() && !inBody2.IsDynamic())
+		&& !(inBody1.IsKinematic() && inBody2.IsSensor())
+		&& !(inBody2.IsKinematic() && inBody1.IsSensor()))
+		return false;
+
+	// Check that body 1 is active
+	uint32 body1_index_in_active_bodies = inBody1.GetIndexInActiveBodiesInternal();
+	JPH_ASSERT(!inBody1.IsStatic() && body1_index_in_active_bodies != Body::cInactiveIndex, "This function assumes that Body 1 is active");
+
+	// If the pair A, B collides we need to ensure that the pair B, A does not collide or else we will handle the collision twice.
+	// If A is the same body as B we don't want to collide (1)
+	// If A is dynamic / kinematic and B is static we should collide (2)
+	// If A is dynamic / kinematic and B is dynamic / kinematic we should only collide if
+	//	- A is active and B is not active (3)
+	//	- A is active and B will become active during this simulation step (4)
+	//	- A is active and B is active, we require a condition that makes A, B collide and B, A not (5)
+	//
+	// In order to implement this we use the index in the active body list and make use of the fact that
+	// a body not in the active list has Body.Index = 0xffffffff which is the highest possible value for an uint32.
+	//
+	// Because we know that A is active we know that A.Index != 0xffffffff:
+	// (1) Because A.Index != 0xffffffff, if A.Index = B.Index then A = B, so to collide A.Index != B.Index
+	// (2) A.Index != 0xffffffff, B.Index = 0xffffffff (because it's static and cannot be in the active list), so to collide A.Index != B.Index
+	// (3) A.Index != 0xffffffff, B.Index = 0xffffffff (because it's not yet active), so to collide A.Index != B.Index
+	// (4) A.Index != 0xffffffff, B.Index = 0xffffffff currently. But it can activate during the Broad/NarrowPhase step at which point it
+	//     will be added to the end of the active list which will make B.Index > A.Index (this holds only true when we don't deactivate
+	//     bodies during the Broad/NarrowPhase step), so to collide A.Index < B.Index.
+	// (5) As tie breaker we can use the same condition A.Index < B.Index to collide, this means that if A, B collides then B, A won't
+	static_assert(Body::cInactiveIndex == 0xffffffff, "The algorithm below uses this value");
+	if (!inBody2.IsSoftBody() && body1_index_in_active_bodies >= inBody2.GetIndexInActiveBodiesInternal())
+		return false;
+	JPH_ASSERT(inBody1.GetID() != inBody2.GetID(), "Read the comment above, A and B are the same body which should not be possible!");
+
+	// Check collision group filter
+	if (!inBody1.GetCollisionGroup().CanCollide(inBody2.GetCollisionGroup()))
+		return false;
+
+	return true;
+}
+
+void Body::AddRotationStep(Vec3Arg inAngularVelocityTimesDeltaTime)
+{
+	JPH_ASSERT(IsRigidBody());
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::ReadWrite));
+
+	// This used to use the equation: d/dt R(t) = 1/2 * w(t) * R(t) so that R(t + dt) = R(t) + 1/2 * w(t) * R(t) * dt
+	// See: Appendix B of An Introduction to Physically Based Modeling: Rigid Body Simulation II-Nonpenetration Constraints
+	// URL: https://www.cs.cmu.edu/~baraff/sigcourse/notesd2.pdf
+	// But this is a first order approximation and does not work well for kinematic ragdolls that are driven to a new
+	// pose if the poses differ enough. So now we split w(t) * dt into an axis and angle part and create a quaternion with it.
+	// Note that the resulting quaternion is normalized since otherwise numerical drift will eventually make the rotation non-normalized.
+	float len = inAngularVelocityTimesDeltaTime.Length();
+	if (len > 1.0e-6f)
+	{
+		mRotation = (Quat::sRotation(inAngularVelocityTimesDeltaTime / len, len) * mRotation).Normalized();
+		JPH_ASSERT(!mRotation.IsNaN());
+	}
+}
+
+void Body::SubRotationStep(Vec3Arg inAngularVelocityTimesDeltaTime)
+{
+	JPH_ASSERT(IsRigidBody());
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::ReadWrite));
+
+	// See comment at Body::AddRotationStep
+	float len = inAngularVelocityTimesDeltaTime.Length();
+	if (len > 1.0e-6f)
+	{
+		mRotation = (Quat::sRotation(inAngularVelocityTimesDeltaTime / len, -len) * mRotation).Normalized();
+		JPH_ASSERT(!mRotation.IsNaN());
+	}
+}
+
+Vec3 Body::GetWorldSpaceSurfaceNormal(const SubShapeID &inSubShapeID, RVec3Arg inPosition) const
+{
+	RMat44 inv_com = GetInverseCenterOfMassTransform();
+	return inv_com.Multiply3x3Transposed(mShape->GetSurfaceNormal(inSubShapeID, Vec3(inv_com * inPosition))).Normalized();
+}
+
+Mat44 Body::GetInverseInertia() const
+{
+	JPH_ASSERT(IsDynamic());
+
+	return GetMotionProperties()->GetInverseInertiaForRotation(Mat44::sRotation(mRotation));
+}
+
+void Body::AddForce(Vec3Arg inForce, RVec3Arg inPosition)
+{
+	AddForce(inForce);
+	AddTorque(Vec3(inPosition - mPosition).Cross(inForce));
+}
+
+void Body::AddImpulse(Vec3Arg inImpulse)
+{
+	JPH_ASSERT(IsDynamic());
+
+	SetLinearVelocityClamped(mMotionProperties->GetLinearVelocity() + inImpulse * mMotionProperties->GetInverseMass());
+}
+
+void Body::AddImpulse(Vec3Arg inImpulse, RVec3Arg inPosition)
+{
+	JPH_ASSERT(IsDynamic());
+
+	SetLinearVelocityClamped(mMotionProperties->GetLinearVelocity() + inImpulse * mMotionProperties->GetInverseMass());
+
+	SetAngularVelocityClamped(mMotionProperties->GetAngularVelocity() + mMotionProperties->MultiplyWorldSpaceInverseInertiaByVector(mRotation, Vec3(inPosition - mPosition).Cross(inImpulse)));
+}
+
+void Body::AddAngularImpulse(Vec3Arg inAngularImpulse)
+{
+	JPH_ASSERT(IsDynamic());
+
+	SetAngularVelocityClamped(mMotionProperties->GetAngularVelocity() + mMotionProperties->MultiplyWorldSpaceInverseInertiaByVector(mRotation, inAngularImpulse));
+}
+
+void Body::GetSleepTestPoints(RVec3 *outPoints) const
+{
+	JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess(), BodyAccess::EAccess::Read));
+
+	// Center of mass is the first position
+	outPoints[0] = mPosition;
+
+	// The second and third position are on the largest axis of the bounding box
+	Vec3 extent = mShape->GetLocalBounds().GetExtent();
+	int lowest_component = extent.GetLowestComponentIndex();
+	Mat44 rotation = Mat44::sRotation(mRotation);
+	switch (lowest_component)
+	{
+	case 0:
+		outPoints[1] = mPosition + extent.GetY() * rotation.GetColumn3(1);
+		outPoints[2] = mPosition + extent.GetZ() * rotation.GetColumn3(2);
+		break;
+
+	case 1:
+		outPoints[1] = mPosition + extent.GetX() * rotation.GetColumn3(0);
+		outPoints[2] = mPosition + extent.GetZ() * rotation.GetColumn3(2);
+		break;
+
+	case 2:
+		outPoints[1] = mPosition + extent.GetX() * rotation.GetColumn3(0);
+		outPoints[2] = mPosition + extent.GetY() * rotation.GetColumn3(1);
+		break;
+
+	default:
+		JPH_ASSERT(false);
+		break;
+	}
+}
+
+void Body::ResetSleepTimer()
+{
+	RVec3 points[3];
+	GetSleepTestPoints(points);
+	mMotionProperties->ResetSleepTestSpheres(points);
+}
+
+JPH_NAMESPACE_END