283 lines
8.8 KiB
C++
283 lines
8.8 KiB
C++
// Redistribution and use in source and binary forms, with or without
|
|
// modification, are permitted provided that the following conditions
|
|
// are met:
|
|
// * Redistributions of source code must retain the above copyright
|
|
// notice, this list of conditions and the following disclaimer.
|
|
// * Redistributions in binary form must reproduce the above copyright
|
|
// notice, this list of conditions and the following disclaimer in the
|
|
// documentation and/or other materials provided with the distribution.
|
|
// * Neither the name of NVIDIA CORPORATION nor the names of its
|
|
// contributors may be used to endorse or promote products derived
|
|
// from this software without specific prior written permission.
|
|
//
|
|
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
|
|
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
|
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
|
// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
|
|
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
|
|
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
|
|
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
|
|
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
|
|
// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
|
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
|
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|
//
|
|
// Copyright (c) 2008-2025 NVIDIA Corporation. All rights reserved.
|
|
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
|
|
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
|
|
|
|
#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
|
|
PxReal residualVelIter;
|
|
private:
|
|
union
|
|
{
|
|
PxU32 useAngularError; //Use only the most significant bit (which corresponds to the float's sign bit)
|
|
PxReal residualPosIter;
|
|
};
|
|
public:
|
|
|
|
void setSolverConstants(const Constraint1dSolverConstantsTGS& desc)
|
|
{
|
|
biasScale = desc.biasScale;
|
|
error = desc.error;
|
|
velTarget = desc.targetVel;
|
|
velMultiplier = desc.velMultiplier;
|
|
}
|
|
|
|
|
|
PX_FORCE_INLINE PxU32 setBit(PxU32 value, PxU32 bitLocation, bool bitState)
|
|
{
|
|
if (bitState)
|
|
return value | (1 << bitLocation);
|
|
else
|
|
return value & (~(1 << bitLocation));
|
|
}
|
|
|
|
PX_FORCE_INLINE void setUseAngularError(bool b)
|
|
{
|
|
useAngularError = setBit(useAngularError, 31, b);
|
|
}
|
|
|
|
PX_FORCE_INLINE PxReal getUseAngularError() const
|
|
{
|
|
return (useAngularError & 0x80000000) ? 1.0f : 0.0f;
|
|
}
|
|
|
|
PX_FORCE_INLINE void setPositionIterationResidual(PxReal residual)
|
|
{
|
|
bool b = getUseAngularError();
|
|
residualPosIter = residual;
|
|
setUseAngularError(b);
|
|
}
|
|
|
|
PX_FORCE_INLINE PxReal getPositionIterationResidual() const
|
|
{
|
|
return PxAbs(residualPosIter);
|
|
}
|
|
|
|
PX_FORCE_INLINE void setVelocityIterationResidual(PxReal residual)
|
|
{
|
|
residualVelIter = residual;
|
|
}
|
|
|
|
} PX_ALIGN_SUFFIX(16);
|
|
|
|
struct SolverConstraint1DExtStep : public SolverConstraint1DStep
|
|
{
|
|
public:
|
|
Cm::SpatialVectorV deltaVA;
|
|
Cm::SpatialVectorV 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.setUseAngularError(true);
|
|
c.residualVelIter = 0.0f;
|
|
c.setPositionIterationResidual(0.0f);
|
|
}
|
|
|
|
}//namespace Dy
|
|
}
|
|
|
|
#endif
|