@bloomengine/engine 0.3.3 → 0.4.1

package/native/third_party/JoltPhysics/Jolt/Physics/Constraints/ConeConstraint.cpp

@@ -1,246 +0,0 @@
-// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
-// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
-// SPDX-License-Identifier: MIT
-
-#include <Jolt/Jolt.h>
-
-#include <Jolt/Physics/Constraints/ConeConstraint.h>
-#include <Jolt/Physics/Body/Body.h>
-#include <Jolt/ObjectStream/TypeDeclarations.h>
-#include <Jolt/Core/StreamIn.h>
-#include <Jolt/Core/StreamOut.h>
-#ifdef JPH_DEBUG_RENDERER
-	#include <Jolt/Renderer/DebugRenderer.h>
-#endif // JPH_DEBUG_RENDERER
-
-JPH_NAMESPACE_BEGIN
-
-JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(ConeConstraintSettings)
-{
-	JPH_ADD_BASE_CLASS(ConeConstraintSettings, TwoBodyConstraintSettings)
-
-	JPH_ADD_ENUM_ATTRIBUTE(ConeConstraintSettings, mSpace)
-	JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mPoint1)
-	JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mTwistAxis1)
-	JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mPoint2)
-	JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mTwistAxis2)
-	JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mHalfConeAngle)
-}
-
-void ConeConstraintSettings::SaveBinaryState(StreamOut &inStream) const
-{
-	ConstraintSettings::SaveBinaryState(inStream);
-
-	inStream.Write(mSpace);
-	inStream.Write(mPoint1);
-	inStream.Write(mTwistAxis1);
-	inStream.Write(mPoint2);
-	inStream.Write(mTwistAxis2);
-	inStream.Write(mHalfConeAngle);
-}
-
-void ConeConstraintSettings::RestoreBinaryState(StreamIn &inStream)
-{
-	ConstraintSettings::RestoreBinaryState(inStream);
-
-	inStream.Read(mSpace);
-	inStream.Read(mPoint1);
-	inStream.Read(mTwistAxis1);
-	inStream.Read(mPoint2);
-	inStream.Read(mTwistAxis2);
-	inStream.Read(mHalfConeAngle);
-}
-
-TwoBodyConstraint *ConeConstraintSettings::Create(Body &inBody1, Body &inBody2) const
-{
-	return new ConeConstraint(inBody1, inBody2, *this);
-}
-
-ConeConstraint::ConeConstraint(Body &inBody1, Body &inBody2, const ConeConstraintSettings &inSettings) :
-	TwoBodyConstraint(inBody1, inBody2, inSettings)
-{
-	// Store limits
-	SetHalfConeAngle(inSettings.mHalfConeAngle);
-
-	// Initialize rotation axis to perpendicular of twist axis in case the angle between the twist axis is 0 in the first frame
-	mWorldSpaceRotationAxis = inSettings.mTwistAxis1.GetNormalizedPerpendicular();
-
-	if (inSettings.mSpace == EConstraintSpace::WorldSpace)
-	{
-		// If all properties were specified in world space, take them to local space now
-		RMat44 inv_transform1 = inBody1.GetInverseCenterOfMassTransform();
-		mLocalSpacePosition1 = Vec3(inv_transform1 * inSettings.mPoint1);
-		mLocalSpaceTwistAxis1 = inv_transform1.Multiply3x3(inSettings.mTwistAxis1);
-
-		RMat44 inv_transform2 = inBody2.GetInverseCenterOfMassTransform();
-		mLocalSpacePosition2 = Vec3(inv_transform2 * inSettings.mPoint2);
-		mLocalSpaceTwistAxis2 = inv_transform2.Multiply3x3(inSettings.mTwistAxis2);
-	}
-	else
-	{
-		// Properties already in local space
-		mLocalSpacePosition1 = Vec3(inSettings.mPoint1);
-		mLocalSpacePosition2 = Vec3(inSettings.mPoint2);
-		mLocalSpaceTwistAxis1 = inSettings.mTwistAxis1;
-		mLocalSpaceTwistAxis2 = inSettings.mTwistAxis2;
-
-		// If they were in local space, we need to take the initial rotation axis to world space
-		mWorldSpaceRotationAxis = inBody1.GetRotation() * mWorldSpaceRotationAxis;
-	}
-}
-
-void ConeConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM)
-{
-	if (mBody1->GetID() == inBodyID)
-		mLocalSpacePosition1 -= inDeltaCOM;
-	else if (mBody2->GetID() == inBodyID)
-		mLocalSpacePosition2 -= inDeltaCOM;
-}
-
-void ConeConstraint::CalculateRotationConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2)
-{
-	// Rotation is along the cross product of both twist axis
-	Vec3 twist1 = inRotation1.Multiply3x3(mLocalSpaceTwistAxis1);
-	Vec3 twist2 = inRotation2.Multiply3x3(mLocalSpaceTwistAxis2);
-
-	// Calculate dot product between twist axis, if it's smaller than the cone angle we need to correct
-	mCosTheta = twist1.Dot(twist2);
-	if (mCosTheta < mCosHalfConeAngle)
-	{
-		// Rotation axis is defined by the two twist axis
-		Vec3 rot_axis = twist2.Cross(twist1);
-
-		// If we can't find a rotation axis because the twist is too small, we'll use last frame's rotation axis
-		float len = rot_axis.Length();
-		if (len > 0.0f)
-			mWorldSpaceRotationAxis = rot_axis / len;
-
-		mAngleConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, mWorldSpaceRotationAxis);
-	}
-	else
-		mAngleConstraintPart.Deactivate();
-}
-
-void ConeConstraint::SetupVelocityConstraint(float inDeltaTime)
-{
-	Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation());
-	Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation());
-	mPointConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, mLocalSpacePosition1, *mBody2, rotation2, mLocalSpacePosition2);
-	CalculateRotationConstraintProperties(rotation1, rotation2);
-}
-
-void ConeConstraint::ResetWarmStart()
-{
-	mPointConstraintPart.Deactivate();
-	mAngleConstraintPart.Deactivate();
-}
-
-void ConeConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio)
-{
-	// Warm starting: Apply previous frame impulse
-	mPointConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
-	mAngleConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio);
-}
-
-bool ConeConstraint::SolveVelocityConstraint(float inDeltaTime)
-{
-	bool pos = mPointConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2);
-
-	bool rot = false;
-	if (mAngleConstraintPart.IsActive())
-		rot = mAngleConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceRotationAxis, 0, FLT_MAX);
-
-	return pos || rot;
-}
-
-bool ConeConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte)
-{
-	mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), mLocalSpacePosition1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2);
-	bool pos = mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte);
-
-	bool rot = false;
-	CalculateRotationConstraintProperties(Mat44::sRotation(mBody1->GetRotation()), Mat44::sRotation(mBody2->GetRotation()));
-	if (mAngleConstraintPart.IsActive())
-		rot = mAngleConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mCosTheta - mCosHalfConeAngle, inBaumgarte);
-
-	return pos || rot;
-}
-
-#ifdef JPH_DEBUG_RENDERER
-void ConeConstraint::DrawConstraint(DebugRenderer *inRenderer) const
-{
-	RMat44 transform1 = mBody1->GetCenterOfMassTransform();
-	RMat44 transform2 = mBody2->GetCenterOfMassTransform();
-
-	RVec3 p1 = transform1 * mLocalSpacePosition1;
-	RVec3 p2 = transform2 * mLocalSpacePosition2;
-
-	// Draw constraint
-	inRenderer->DrawMarker(p1, Color::sRed, 0.1f);
-	inRenderer->DrawMarker(p2, Color::sGreen, 0.1f);
-
-	// Draw twist axis
-	inRenderer->DrawLine(p1, p1 + mDrawConstraintSize * transform1.Multiply3x3(mLocalSpaceTwistAxis1), Color::sRed);
-	inRenderer->DrawLine(p2, p2 + mDrawConstraintSize * transform2.Multiply3x3(mLocalSpaceTwistAxis2), Color::sGreen);
-}
-
-void ConeConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const
-{
-	// Get constraint properties in world space
-	RMat44 transform1 = mBody1->GetCenterOfMassTransform();
-	RVec3 position1 = transform1 * mLocalSpacePosition1;
-	Vec3 twist_axis1 = transform1.Multiply3x3(mLocalSpaceTwistAxis1);
-	Vec3 normal_axis1 = transform1.Multiply3x3(mLocalSpaceTwistAxis1.GetNormalizedPerpendicular());
-
-	inRenderer->DrawOpenCone(position1, twist_axis1, normal_axis1, ACos(mCosHalfConeAngle), mDrawConstraintSize * mCosHalfConeAngle, Color::sPurple, DebugRenderer::ECastShadow::Off);
-}
-#endif // JPH_DEBUG_RENDERER
-
-void ConeConstraint::SaveState(StateRecorder &inStream) const
-{
-	TwoBodyConstraint::SaveState(inStream);
-
-	mPointConstraintPart.SaveState(inStream);
-	mAngleConstraintPart.SaveState(inStream);
-	inStream.Write(mWorldSpaceRotationAxis); // When twist is too small, the rotation is used from last frame so we need to store it
-}
-
-void ConeConstraint::RestoreState(StateRecorder &inStream)
-{
-	TwoBodyConstraint::RestoreState(inStream);
-
-	mPointConstraintPart.RestoreState(inStream);
-	mAngleConstraintPart.RestoreState(inStream);
-	inStream.Read(mWorldSpaceRotationAxis);
-}
-
-Ref<ConstraintSettings> ConeConstraint::GetConstraintSettings() const
-{
-	ConeConstraintSettings *settings = new ConeConstraintSettings;
-	ToConstraintSettings(*settings);
-	settings->mSpace = EConstraintSpace::LocalToBodyCOM;
-	settings->mPoint1 = RVec3(mLocalSpacePosition1);
-	settings->mTwistAxis1 = mLocalSpaceTwistAxis1;
-	settings->mPoint2 = RVec3(mLocalSpacePosition2);
-	settings->mTwistAxis2 = mLocalSpaceTwistAxis2;
-	settings->mHalfConeAngle = ACos(mCosHalfConeAngle);
-	return settings;
-}
-
-Mat44 ConeConstraint::GetConstraintToBody1Matrix() const
-{
-	Vec3 perp = mLocalSpaceTwistAxis1.GetNormalizedPerpendicular();
-	Vec3 perp2 = mLocalSpaceTwistAxis1.Cross(perp);
-	return Mat44(Vec4(mLocalSpaceTwistAxis1, 0), Vec4(perp, 0), Vec4(perp2, 0), Vec4(mLocalSpacePosition1, 1));
-}
-
-Mat44 ConeConstraint::GetConstraintToBody2Matrix() const
-{
-	// Note: Incorrect in rotation around the twist axis (the perpendicular does not match that of body 1),
-	// this should not matter as we're not limiting rotation around the twist axis.
-	Vec3 perp = mLocalSpaceTwistAxis2.GetNormalizedPerpendicular();
-	Vec3 perp2 = mLocalSpaceTwistAxis2.Cross(perp);
-	return Mat44(Vec4(mLocalSpaceTwistAxis2, 0), Vec4(perp, 0), Vec4(perp2, 0), Vec4(mLocalSpacePosition2, 1));
-}
-
-JPH_NAMESPACE_END

package/native/third_party/JoltPhysics/Jolt/Physics/Constraints/ConeConstraint.h

@@ -1,133 +0,0 @@
-// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
-// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
-// SPDX-License-Identifier: MIT
-
-#pragma once
-
-#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>
-#include <Jolt/Physics/Constraints/ConstraintPart/PointConstraintPart.h>
-#include <Jolt/Physics/Constraints/ConstraintPart/AngleConstraintPart.h>
-
-JPH_NAMESPACE_BEGIN
-
-/// Cone constraint settings, used to create a cone constraint
-class JPH_EXPORT ConeConstraintSettings final : public TwoBodyConstraintSettings
-{
-	JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, ConeConstraintSettings)
-
-public:
-	// See: ConstraintSettings::SaveBinaryState
-	virtual void				SaveBinaryState(StreamOut &inStream) const override;
-
-	/// Create an instance of this constraint
-	virtual TwoBodyConstraint *	Create(Body &inBody1, Body &inBody2) const override;
-
-	/// This determines in which space the constraint is setup, all properties below should be in the specified space
-	EConstraintSpace			mSpace = EConstraintSpace::WorldSpace;
-
-	/// Body 1 constraint reference frame (space determined by mSpace)
-	RVec3						mPoint1 = RVec3::sZero();
-	Vec3						mTwistAxis1 = Vec3::sAxisX();
-
-	/// Body 2 constraint reference frame (space determined by mSpace)
-	RVec3						mPoint2 = RVec3::sZero();
-	Vec3						mTwistAxis2 = Vec3::sAxisX();
-
-	/// Half of maximum angle between twist axis of body 1 and 2
-	float						mHalfConeAngle = 0.0f;
-
-protected:
-	// See: ConstraintSettings::RestoreBinaryState
-	virtual void				RestoreBinaryState(StreamIn &inStream) override;
-};
-
-/// A cone constraint constraints 2 bodies to a single point and limits the swing between the twist axis within a cone:
-///
-/// t1 . t2 <= cos(theta)
-///
-/// Where:
-///
-/// t1 = twist axis of body 1.
-/// t2 = twist axis of body 2.
-/// theta = half cone angle (angle from the principal axis of the cone to the edge).
-///
-/// Calculating the Jacobian:
-///
-/// Constraint equation:
-///
-/// C = t1 . t2 - cos(theta)
-///
-/// Derivative:
-///
-/// d/dt C = d/dt (t1 . t2) = (d/dt t1) . t2 + t1 . (d/dt t2) = (w1 x t1) . t2 + t1 . (w2 x t2) = (t1 x t2) . w1 + (t2 x t1) . w2
-///
-/// d/dt C = J v = [0, -t2 x t1, 0, t2 x t1] [v1, w1, v2, w2]
-///
-/// Where J is the Jacobian.
-///
-/// Note that this is the exact same equation as used in AngleConstraintPart if we use t2 x t1 as the world space axis
-class JPH_EXPORT ConeConstraint final : public TwoBodyConstraint
-{
-public:
-	JPH_OVERRIDE_NEW_DELETE
-
-	/// Construct cone constraint
-								ConeConstraint(Body &inBody1, Body &inBody2, const ConeConstraintSettings &inSettings);
-
-	// Generic interface of a constraint
-	virtual EConstraintSubType	GetSubType() const override					{ return EConstraintSubType::Cone; }
-	virtual void				NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override;
-	virtual void				SetupVelocityConstraint(float inDeltaTime) override;
-	virtual void				ResetWarmStart() override;
-	virtual void				WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override;
-	virtual bool				SolveVelocityConstraint(float inDeltaTime) override;
-	virtual bool				SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override;
-#ifdef JPH_DEBUG_RENDERER
-	virtual void				DrawConstraint(DebugRenderer *inRenderer) const override;
-	virtual void				DrawConstraintLimits(DebugRenderer *inRenderer) const override;
-#endif // JPH_DEBUG_RENDERER
-	virtual void				SaveState(StateRecorder &inStream) const override;
-	virtual void				RestoreState(StateRecorder &inStream) override;
-	virtual Ref<ConstraintSettings> GetConstraintSettings() const override;
-
-	// See: TwoBodyConstraint
-	virtual Mat44				GetConstraintToBody1Matrix() const override;
-	virtual Mat44				GetConstraintToBody2Matrix() const override;
-
-	/// Update maximum angle between body 1 and 2 (see ConeConstraintSettings)
-	void						SetHalfConeAngle(float inHalfConeAngle)		{ JPH_ASSERT(inHalfConeAngle >= 0.0f && inHalfConeAngle <= JPH_PI); mCosHalfConeAngle = Cos(inHalfConeAngle); }
-	float						GetCosHalfConeAngle() const					{ return mCosHalfConeAngle; }
-
-	///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint)
-	inline Vec3					GetTotalLambdaPosition() const				{ return mPointConstraintPart.GetTotalLambda(); }
-	inline float				GetTotalLambdaRotation() const				{ return mAngleConstraintPart.GetTotalLambda(); }
-
-private:
-	// Internal helper function to calculate the values below
-	void						CalculateRotationConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2);
-
-	// CONFIGURATION PROPERTIES FOLLOW
-
-	// Local space constraint positions
-	Vec3						mLocalSpacePosition1;
-	Vec3						mLocalSpacePosition2;
-
-	// Local space constraint axis
-	Vec3						mLocalSpaceTwistAxis1;
-	Vec3						mLocalSpaceTwistAxis2;
-
-	// Angular limits
-	float						mCosHalfConeAngle;
-
-	// RUN TIME PROPERTIES FOLLOW
-
-	// Axis and angle of rotation between the two bodies
-	Vec3						mWorldSpaceRotationAxis;
-	float						mCosTheta;
-
-	// The constraint parts
-	PointConstraintPart			mPointConstraintPart;
-	AngleConstraintPart			mAngleConstraintPart;
-};
-
-JPH_NAMESPACE_END

package/native/third_party/JoltPhysics/Jolt/Physics/Constraints/Constraint.cpp

@@ -1,73 +0,0 @@
-// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
-// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
-// SPDX-License-Identifier: MIT
-
-#include <Jolt/Jolt.h>
-
-#include <Jolt/Physics/Constraints/Constraint.h>
-#include <Jolt/Physics/StateRecorder.h>
-#include <Jolt/ObjectStream/TypeDeclarations.h>
-#include <Jolt/Core/StreamUtils.h>
-
-JPH_NAMESPACE_BEGIN
-
-JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(ConstraintSettings)
-{
-	JPH_ADD_BASE_CLASS(ConstraintSettings, SerializableObject)
-
-	JPH_ADD_ATTRIBUTE(ConstraintSettings, mEnabled)
-	JPH_ADD_ATTRIBUTE(ConstraintSettings, mDrawConstraintSize)
-	JPH_ADD_ATTRIBUTE(ConstraintSettings, mConstraintPriority)
-	JPH_ADD_ATTRIBUTE(ConstraintSettings, mNumVelocityStepsOverride)
-	JPH_ADD_ATTRIBUTE(ConstraintSettings, mNumPositionStepsOverride)
-	JPH_ADD_ATTRIBUTE(ConstraintSettings, mUserData)
-}
-
-void ConstraintSettings::SaveBinaryState(StreamOut &inStream) const
-{
-	inStream.Write(GetRTTI()->GetHash());
-	inStream.Write(mEnabled);
-	inStream.Write(mDrawConstraintSize);
-	inStream.Write(mConstraintPriority);
-	inStream.Write(mNumVelocityStepsOverride);
-	inStream.Write(mNumPositionStepsOverride);
-}
-
-void ConstraintSettings::RestoreBinaryState(StreamIn &inStream)
-{
-	// Type hash read by sRestoreFromBinaryState
-	inStream.Read(mEnabled);
-	inStream.Read(mDrawConstraintSize);
-	inStream.Read(mConstraintPriority);
-	inStream.Read(mNumVelocityStepsOverride);
-	inStream.Read(mNumPositionStepsOverride);
-}
-
-ConstraintSettings::ConstraintResult ConstraintSettings::sRestoreFromBinaryState(StreamIn &inStream)
-{
-	return StreamUtils::RestoreObject<ConstraintSettings>(inStream, &ConstraintSettings::RestoreBinaryState);
-}
-
-void Constraint::SaveState(StateRecorder &inStream) const
-{
-	inStream.Write(mEnabled);
-}
-
-void Constraint::RestoreState(StateRecorder &inStream)
-{
-	inStream.Read(mEnabled);
-}
-
-void Constraint::ToConstraintSettings(ConstraintSettings &outSettings) const
-{
-	outSettings.mEnabled = mEnabled;
-	outSettings.mConstraintPriority = mConstraintPriority;
-	outSettings.mNumVelocityStepsOverride = mNumVelocityStepsOverride;
-	outSettings.mNumPositionStepsOverride = mNumPositionStepsOverride;
-	outSettings.mUserData = mUserData;
-#ifdef JPH_DEBUG_RENDERER
-	outSettings.mDrawConstraintSize = mDrawConstraintSize;
-#endif // JPH_DEBUG_RENDERER
-}
-
-JPH_NAMESPACE_END