Skip to content

A fast 2D and 3D game engine — TypeScript SDK, C++/WebAssembly core, visual editor. Ship one project to web, desktop, WeChat MiniGames, playable ads, and native Android / iOS.

6.3 KB
Raw Blame
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
// SPDX-FileCopyrightText: Copyright (c) 2008-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
// SPDX-License-Identifier: Apache-2.0

#ifndef DY_SOLVER_CONSTRAINT_1D_STEP_H
#define DY_SOLVER_CONSTRAINT_1D_STEP_H

#include "CmSpatialVector.h"
#include "foundation/PxVec3.h"
#include "DySolverConstraintTypes.h"
#include "PxConstraintDesc.h"
#include "DyCpuGpu1dConstraint.h"


namespace physx
{
	namespace Sc
	{
		class ShapeInteraction;
	}
	namespace Dy
	{
		struct SolverContactHeaderStep
		{
			enum DySolverContactFlags
			{
				eHAS_FORCE_THRESHOLDS = 0x1
			};

			PxU8	type;					//Note: mType should be first as the solver expects a type in the first byte.
			PxU8	flags;
			PxU8	numNormalConstr;
			PxU8	numFrictionConstr;					//4

			PxReal	angDom0;							//8
			PxReal	angDom1;							//12
			PxReal	invMass0;							//16

			aos::Vec4V   staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W;		//32
			PxVec3	normal;															//48
			
			PxReal	maxPenBias;														//52
			PxReal	invMass1;														//56
			PxReal	minNormalForce;													//60
			PxU32 broken;															//64
			PxU8* frictionBrokenWritebackByte;										//68	72
			Sc::ShapeInteraction* shapeInteraction;									//72	80
#if !PX_P64_FAMILY
			PxU32 pad[2];															//80
#endif		

			PX_FORCE_INLINE aos::FloatV getStaticFriction() const { return aos::V4GetX(staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W); }
			PX_FORCE_INLINE aos::FloatV getDynamicFriction() const { return aos::V4GetY(staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W); }
			PX_FORCE_INLINE aos::FloatV getDominance0() const { return aos::V4GetZ(staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W); }
			PX_FORCE_INLINE aos::FloatV getDominance1() const { return aos::V4GetW(staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W); }
		};

		struct SolverContactPointStep
		{
			PxVec3 raXnI;
			PxF32 separation;
			PxVec3 rbXnI;
			PxF32 velMultiplier;
			PxF32 targetVelocity;
			PxF32 biasCoefficient;
			PxF32 recipResponse;
			PxF32 maxImpulse;
		};

		struct SolverContactPointStepExt : public SolverContactPointStep
		{
			aos::Vec3V linDeltaVA;
			aos::Vec3V linDeltaVB;
			aos::Vec3V angDeltaVA;
			aos::Vec3V angDeltaVB;
		};

		struct SolverContactFrictionStep
		{
			aos::Vec4V normalXYZ_ErrorW;
			aos::Vec4V raXnI_targetVelW;
			aos::Vec4V rbXnI_velMultiplierW;
			PxReal biasScale;
			PxReal appliedForce;
			PxReal frictionScale;
			PxU32 pad[1];

			PX_FORCE_INLINE void setAppliedForce(const aos::FloatV f) { aos::FStore(f, &appliedForce); }

			PX_FORCE_INLINE aos::Vec3V getNormal() const { return aos::Vec3V_From_Vec4V(normalXYZ_ErrorW); }
			PX_FORCE_INLINE aos::FloatV getAppliedForce() const { return aos::FLoad(appliedForce); }
		};
		PX_COMPILE_TIME_ASSERT(sizeof(SolverContactFrictionStep) % 16 == 0);

		struct SolverContactFrictionStepExt : public SolverContactFrictionStep
		{
			aos::Vec3V linDeltaVA;
			aos::Vec3V linDeltaVB;
			aos::Vec3V angDeltaVA;
			aos::Vec3V angDeltaVB;
		};

		struct SolverConstraint1DHeaderStep
		{
			PxU8	type;			// enum SolverConstraintType - must be first byte
			PxU8	count;			// count of following 1D constraints
			PxU8	dominance;
			PxU8	breakable;		// indicate whether this constraint is breakable or not						
			PxReal	linBreakImpulse;
			PxReal	angBreakImpulse;
			PxReal	invMass0D0;

			PxVec3	body0WorldOffset;
			PxReal	invMass1D1;

			PxVec3	rAWorld;
			PxReal linearInvMassScale0;		// only used by articulations

			PxVec3	rBWorld;
			PxReal	angularInvMassScale0;

			PxReal	linearInvMassScale1;		// only used by articulations
			PxReal	angularInvMassScale1;
			PxU32	pad[2];

			//Ortho axes for body 0, recipResponse in W component
			PxVec4    angOrthoAxis0_recipResponseW[3];
			//Ortho axes for body 1, error of body in W component
			PxVec4    angOrthoAxis1_Error[3];
		};
		PX_COMPILE_TIME_ASSERT(PX_OFFSET_OF(SolverConstraint1DHeaderStep, angOrthoAxis0_recipResponseW) % 16 == 0);


		PX_FORCE_INLINE void init(SolverConstraint1DHeaderStep& h,
			PxU8 count,
			bool isExtended,
			const PxConstraintInvMassScale& ims)
		{
			h.type = PxU8(isExtended ? DY_SC_TYPE_EXT_1D : DY_SC_TYPE_RB_1D);
			h.count = count;
			h.dominance = 0;
			h.linearInvMassScale0 = ims.linear0;
			h.angularInvMassScale0 = ims.angular0;
			h.linearInvMassScale1 = -ims.linear1;
			h.angularInvMassScale1 = -ims.angular1;
		}


		PX_ALIGN_PREFIX(16)
		struct SolverConstraint1DStep
		{
		public:
			PxVec3		lin0;					//!< linear velocity projection (body 0)	
			PxReal		error;					//!< constraint error term - must be scaled by biasScale. Can be adjusted at run-time

			PxVec3		lin1;					//!< linear velocity projection (body 1)
			PxReal		biasScale;				//!< constraint constant bias scale. Constant

			PxVec3		ang0;					//!< angular velocity projection (body 0)
			PxReal		velMultiplier;			//!< constraint velocity multiplier

			PxVec3		ang1;					//!< angular velocity projection (body 1)
			PxReal		velTarget;				//!< Scaled target velocity of the constraint drive

			PxReal		minImpulse;				//!< Lower bound on impulse magnitude	 
			PxReal		maxImpulse;				//!< Upper bound on impulse magnitude
			PxReal		appliedForce;			//!< applied force to correct velocity+bias
			PxReal		maxBias;

			PxU32		flags;
			PxReal		recipResponse;			//Constant. Only used for articulations;
			PxReal		angularErrorScale;		//Constant
			PxU32		pad;

			void setSolverConstants(const Constraint1dSolverConstantsTGS& desc)
			{
				biasScale = desc.biasScale;
				error = desc.error;
				velTarget = desc.targetVel;
				velMultiplier = desc.velMultiplier;
			}
		} PX_ALIGN_SUFFIX(16);

		struct SolverConstraint1DExtStep : public SolverConstraint1DStep
		{
		public:
			Cm::SpatialVector deltaVA_;
			Cm::SpatialVector deltaVB_;
		};

		PX_FORCE_INLINE void init(SolverConstraint1DStep& c,
			const PxVec3& _linear0, const PxVec3& _linear1,
			const PxVec3& _angular0, const PxVec3& _angular1,
			PxReal _minImpulse, PxReal _maxImpulse)
		{
			PX_ASSERT(_linear0.isFinite());
			PX_ASSERT(_linear1.isFinite());
			c.lin0 = _linear0;
			c.lin1 = _linear1;
			c.ang0 = _angular0;
			c.ang1 = _angular1;
			c.minImpulse = _minImpulse;
			c.maxImpulse = _maxImpulse;
			c.flags = 0;
			c.appliedForce = 0;
			c.angularErrorScale = 1.f;
		}

	}//namespace Dy
}

#endif