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.
TypeScript 2d-game-engine3d3d-game-engineandroidcppecsgame-editorgame-enginegamedeviostypescriptwebassemblywebglwebgpuwechat-minigame
// 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_SHARED_H
#define DY_SOLVER_CONSTRAINT_SHARED_H
#include "foundation/PxPreprocessor.h"
#include "foundation/PxVecMath.h"
#include "DySolverBody.h"
#include "DySolverContact.h"
#include "DySolverConstraint1D.h"
#include "DySolverConstraintDesc.h"
#include "foundation/PxUtilities.h"
#include "DyConstraint.h"
#include "foundation/PxAtomic.h"
namespace physx
{
namespace Dy
{
PX_FORCE_INLINE static FloatV solveDynamicContacts(const SolverContactPoint* PX_RESTRICT contacts, PxU32 nbContactPoints, const Vec3VArg contactNormal,
const FloatVArg invMassA, const FloatVArg invMassB, const FloatVArg angDom0, const FloatVArg angDom1, Vec3V& linVel0_, Vec3V& angState0_,
Vec3V& linVel1_, Vec3V& angState1_, PxF32* PX_RESTRICT forceBuffer)
{
Vec3V linVel0 = linVel0_;
Vec3V angState0 = angState0_;
Vec3V linVel1 = linVel1_;
Vec3V angState1 = angState1_;
FloatV accumulatedNormalImpulse = FZero();
const Vec3V delLinVel0 = V3Scale(contactNormal, invMassA);
const Vec3V delLinVel1 = V3Scale(contactNormal, invMassB);
for(PxU32 i=0;i<nbContactPoints;i++)
{
const SolverContactPoint& c = contacts[i];
PxPrefetchLine(&contacts[i], 128);
const Vec3V raXn = Vec3V_From_Vec4V(c.raXn_velMultiplierW);
const Vec3V rbXn = Vec3V_From_Vec4V(c.rbXn_maxImpulseW);
const FloatV appliedForce = FLoad(forceBuffer[i]);
const FloatV velMultiplier = c.getVelMultiplier();
const FloatV impulseMultiplier = c.getImpulseMultiplier();
/*const FloatV targetVel = c.getTargetVelocity();
const FloatV nScaledBias = c.getScaledBias();*/
const FloatV maxImpulse = c.getMaxImpulse();
//Compute the normal velocity of the constraint.
const Vec3V v0 = V3MulAdd(linVel0, contactNormal, V3Mul(angState0, raXn));
const Vec3V v1 = V3MulAdd(linVel1, contactNormal, V3Mul(angState1, rbXn));
const FloatV normalVel = V3SumElems(V3Sub(v0, v1));
const FloatV biasedErr = c.getBiasedErr();//FScaleAdd(targetVel, velMultiplier, nScaledBias);
//KS - clamp the maximum force
const FloatV _deltaF = FMax(FNegScaleSub(normalVel, velMultiplier, biasedErr), FNeg(appliedForce));
const FloatV _newForce = FAdd(FMul(impulseMultiplier, appliedForce), _deltaF);
const FloatV newForce = FMin(_newForce, maxImpulse);
const FloatV deltaF = FSub(newForce, appliedForce);
linVel0 = V3ScaleAdd(delLinVel0, deltaF, linVel0);
linVel1 = V3NegScaleSub(delLinVel1, deltaF, linVel1);
angState0 = V3ScaleAdd(raXn, FMul(deltaF, angDom0), angState0);
angState1 = V3NegScaleSub(rbXn, FMul(deltaF, angDom1), angState1);
FStore(newForce, &forceBuffer[i]);
accumulatedNormalImpulse = FAdd(accumulatedNormalImpulse, newForce);
}
linVel0_ = linVel0;
angState0_ = angState0;
linVel1_ = linVel1;
angState1_ = angState1;
return accumulatedNormalImpulse;
}
PX_FORCE_INLINE static FloatV solveStaticContacts(const SolverContactPoint* PX_RESTRICT contacts, PxU32 nbContactPoints, const Vec3VArg contactNormal,
const FloatVArg invMassA, const FloatVArg angDom0, Vec3V& linVel0_, Vec3V& angState0_, PxF32* PX_RESTRICT forceBuffer)
{
Vec3V linVel0 = linVel0_;
Vec3V angState0 = angState0_;
FloatV accumulatedNormalImpulse = FZero();
const Vec3V delLinVel0 = V3Scale(contactNormal, invMassA);
for(PxU32 i=0;i<nbContactPoints;i++)
{
const SolverContactPoint& c = contacts[i];
PxPrefetchLine(&contacts[i],128);
const Vec3V raXn = Vec3V_From_Vec4V(c.raXn_velMultiplierW);
const FloatV appliedForce = FLoad(forceBuffer[i]);
const FloatV velMultiplier = c.getVelMultiplier();
const FloatV impulseMultiplier = c.getImpulseMultiplier();
/*const FloatV targetVel = c.getTargetVelocity();
const FloatV nScaledBias = c.getScaledBias();*/
const FloatV maxImpulse = c.getMaxImpulse();
const Vec3V v0 = V3MulAdd(linVel0, contactNormal, V3Mul(angState0, raXn));
const FloatV normalVel = V3SumElems(v0);
const FloatV biasedErr = c.getBiasedErr();//FScaleAdd(targetVel, velMultiplier, nScaledBias);
// still lots to do here: using loop pipelining we can interweave this code with the
// above - the code here has a lot of stalls that we would thereby eliminate
const FloatV _deltaF = FMax(FNegScaleSub(normalVel, velMultiplier, biasedErr), FNeg(appliedForce));
const FloatV _newForce = FAdd(FMul(appliedForce, impulseMultiplier), _deltaF);
const FloatV newForce = FMin(_newForce, maxImpulse);
const FloatV deltaF = FSub(newForce, appliedForce);
linVel0 = V3ScaleAdd(delLinVel0, deltaF, linVel0);
angState0 = V3ScaleAdd(raXn, FMul(deltaF, angDom0), angState0);
FStore(newForce, &forceBuffer[i]);
accumulatedNormalImpulse = FAdd(accumulatedNormalImpulse, newForce);
}
linVel0_ = linVel0;
angState0_ = angState0;
return accumulatedNormalImpulse;
}
FloatV solveExtContacts(const SolverContactPointExt* PX_RESTRICT contacts, PxU32 nbContactPoints, const Vec3VArg contactNormal,
Vec3V& linVel0, Vec3V& angVel0,
Vec3V& linVel1, Vec3V& angVel1,
Vec3V& li0, Vec3V& ai0,
Vec3V& li1, Vec3V& ai1,
PxF32* PX_RESTRICT appliedForceBuffer);
}
}
#endif