void FreeMove::processInput(const InputManager& input, float delta)
{
float movAmt = m_speed * delta;
if (input.GetThumbLPosition().y > 0.1f)
Move(Utility::getForward(*getTransform()->getRotation()), movAmt * PxAbs(input.GetThumbLPosition().y));
else if (input.GetThumbLPosition().y < -0.1f)
Move(Utility::getBack(*getTransform()->getRotation()), movAmt * PxAbs(input.GetThumbLPosition().y));
if (input.GetThumbLPosition().x < -0.1f)
Move(Utility::getLeft(*getTransform()->getRotation()), movAmt * PxAbs(input.GetThumbLPosition().x));
else if (input.GetThumbLPosition().x > 0.1f)
Move(Utility::getRight(*getTransform()->getRotation()), movAmt * PxAbs(input.GetThumbLPosition().x));
if (input.KeyDown(m_forwardKey))
Move(Utility::getForward(*getTransform()->getRotation()), movAmt);
if (input.KeyDown(m_backKey))
Move(Utility::getBack(*getTransform()->getRotation()), movAmt);
if (input.KeyDown(m_leftKey))
Move(Utility::getLeft(*getTransform()->getRotation()), movAmt);
if (input.KeyDown(m_rightKey))
Move(Utility::getRight(*getTransform()->getRotation()), movAmt);
if (input.KeyDown(SDLK_r))
{
Move(Utility::getUp(*getTransform()->getRotation()), movAmt);
}
if (input.KeyDown(SDLK_f))
{
Move(Utility::getDown(*getTransform()->getRotation()), movAmt);
}
}
bool PxVehicleTireData::isValid() const
{
PX_CHECK_AND_RETURN_VAL(mFrictionVsSlipGraph[0][0]>=0.0f && mFrictionVsSlipGraph[0][1]>=0.0f, "Illegal values for mFrictionVsSlipGraph[0]", false);
PX_CHECK_AND_RETURN_VAL(mFrictionVsSlipGraph[1][0]>=0.0f && mFrictionVsSlipGraph[1][1]>=0.0f, "Illegal values for mFrictionVsSlipGraph[1]", false);
PX_CHECK_AND_RETURN_VAL(mFrictionVsSlipGraph[2][0]>=0.0f && mFrictionVsSlipGraph[2][1]>=0.0f, "Illegal values for mFrictionVsSlipGraph[2]", false);
PX_CHECK_AND_RETURN_VAL(PxAbs((1.0f/(mFrictionVsSlipGraph[1][0]-mFrictionVsSlipGraph[0][0])) - mFrictionVsSlipGraphRecipx1Minusx0) < 0.001f, "PxVehicleTireData.mFrictionVsSlipGraphRecipx1Minusx0 not set up", false);
PX_CHECK_AND_RETURN_VAL(PxAbs((1.0f/(mFrictionVsSlipGraph[2][0]-mFrictionVsSlipGraph[1][0])) - mFrictionVsSlipGraphRecipx2Minusx1) < 0.001f, "PxVehicleTireData.mFrictionVsSlipGraphRecipx2Minusx1 not set up", false);
return true;
}
bool PxVehicleWheelData::isValid() const
{
PX_CHECK_AND_RETURN_VAL(mRadius>0.0f, "PxVehicleWheelData.mRadius must be greater than zero", false);
PX_CHECK_AND_RETURN_VAL(mWidth>0.0f, "PxVehicleWheelData.mWidth must be greater than zero", false);
PX_CHECK_AND_RETURN_VAL(mMass>0.0f, "PxVehicleWheelData.mMass must be greater than zero", false);
PX_CHECK_AND_RETURN_VAL(mMOI>0.0f, "PxVehicleWheelData.mMOI must be greater than zero", false);
PX_CHECK_AND_RETURN_VAL(mDampingRate>0.0f, "PxVehicleWheelData.mDampingRate must be greater than zero", false);
PX_CHECK_AND_RETURN_VAL(mMaxBrakeTorque>=0.0f, "PxVehicleWheelData.mMaxBrakeTorque must be greater than or equal to zero", false);
PX_CHECK_AND_RETURN_VAL(mMaxHandBrakeTorque>=0.0f, "PxVehicleWheelData.mMaxHandBrakeTorque must be greater than or equal to zero", false);
PX_CHECK_AND_RETURN_VAL(mToeAngle<=PxPi, "PxVehicleWheelData.mToeAngle must be less than Pi", false);
PX_CHECK_AND_RETURN_VAL(PxAbs((1.0f/mRadius) - mRecipRadius) < 0.001f, "PxVehicleWheelData.mRadius and PxVehicleWheelData.mRecipRadius don't match", false);
PX_CHECK_AND_RETURN_VAL(PxAbs((1.0f/mMOI) - mRecipMOI) < 0.001f, "PxVehicleWheelData.mMOI and PxVehicleWheelData.mRecipMOI don't match", false);
return true;
}
void SampleVehicleWayPoints::update(const PxTransform& playerTransform, const PxF32 timestep)
{
//Increment the elapsed time
mTimeElapsed+=timestep;
//Work out the point on the crossing line of the next way-point that is closest to the player.
const PxTransform& nextWayPoint=mWayPoints[mProgress+1];
const PxVec3 v=nextWayPoint.p;
const PxVec3 w=nextWayPoint.q.getBasisVector0();
const PxVec3 p=playerTransform.p;
const PxVec3 pv=p-v;
const PxF32 t=pv.dot(w);
//Test if the player's position is inside the width of the line crossing the next way-point.
if(PxAbs(t) < LINEWIDTH)
{
//Now test if the shortest distance to the next crossing line is smaller than a threshold.
const PxVec3 linePos=v+w*t;
const PxVec3 diff=p-linePos;
const PxF32 dist2=diff.magnitudeSquared();
if(dist2<LINEDISTANCE2)
{
mProgress++;
}
}
if(mProgress == mNumWayPoints-1)
{
mMinTimeElapsed=PxMin(mTimeElapsed, mMinTimeElapsed);
mTimeElapsed=0;
mProgress=0;
}
}
float variableOscillator::updateVariableOscillator(float deltaTime)
{
float returnVal;
float halfRange;
mCumTime += deltaTime;
// has the function crossed a max or a min?
if ((mGoingUp && (mCumTime > (mPeriod / 2.0f))) ||
(!mGoingUp && (mCumTime > mPeriod)))
{
mStartVal = mLastVal;
if (mGoingUp)
{
mEndVal = computeEndVal(mStartVal, mMin);
}
else
{
mEndVal = computeEndVal(mStartVal, mMax);
mCumTime = mCumTime - mPeriod;
}
mGoingUp = !mGoingUp;
}
halfRange = 0.5f * PxAbs(mEndVal - mStartVal);
returnVal = -halfRange * PxCos(mCumTime * PxTwoPi / mPeriod) + halfRange + PxMin(mStartVal, mEndVal);
mLastVal = returnVal;
return(returnVal);
}
float variableOscillator::computeEndVal(float current, float maxOrMin)
{
float target;
float maxDelta;
float quarterVal;
// compute the max range of the oscillator
maxDelta = maxOrMin - current;
// find the 'lower bound' of the oscillator peak
quarterVal = current + (maxDelta / 4.0f);
// get a rand between 0 and 1
target = (float) ::rand() / (float) RAND_MAX;
// scale the rand to the range we want
target = target * PxAbs(quarterVal - maxOrMin);
// add the offset to the scaled random number.
if (current < maxOrMin)
{
target = target + quarterVal;
}
else
{
target = quarterVal - target;
}
return(target);
}
void ApexQuadricSimplifier::computeCost(QuadricEdge& edge)
{
const uint32_t numSteps = 10;
QuadricVertex* qv0 = mVertices[edge.vertexNr[0]];
QuadricVertex* qv1 = mVertices[edge.vertexNr[1]];
edge.cost = FLT_MAX;
edge.lengthSquared = (qv0->pos - qv1->pos).magnitudeSquared();
edge.ratio = -1.0f;
Quadric q;
q = qv0->q + qv1->q;
float sumCost = 0;
for (uint32_t i = 0; i <= numSteps; i++)
{
const float ratio = 1.0f / numSteps * i;
const PxVec3 pos = qv0->pos * (1.0f - ratio) + qv1->pos * ratio;
const float cost = PxAbs(q.outerProduct(pos));
sumCost += cost;
if (cost < edge.cost)
{
edge.cost = cost;
edge.ratio = ratio;
}
}
if (sumCost < 0.0001f)
{
edge.cost = 0;
edge.ratio = 0.5f;
}
}
void conclude1D(const PxcSolverConstraintDesc& desc, PxcSolverContext& cache)
{
PxU8* bPtr = desc.constraint;
//PxU32 length = desc.constraintLength;
PxcSolverConstraint1DHeader* header = reinterpret_cast<PxcSolverConstraint1DHeader *>(bPtr);
PxcSolverConstraint1D* const base = reinterpret_cast<PxcSolverConstraint1D*>(bPtr + sizeof(PxcSolverConstraint1DHeader));
for(PxU32 i=0; i<header->count; i++)
{
PxcSolverConstraint1D& c = base[i];
if(!(c.flags & PXS_SC_FLAG_KEEP_BIAS))
c.bias = 0.0f;
// The impulse produced by the position change, i.e., bias, should be discarded in the last iteration
// step. Hence, the impulse term used for the implicit spring should take only the impulse of the
// velocity change (appliedVForce) into account. Since we want to use the same solver routine for the
// last iteration, we adjust the impulse multiplier such that only the velocity change impulse is
// regarded.
// Note: This is a bit of a hack, i.e., the adjusted impulse multiplier is correct for computing
// the applied total impulse (appliedForce) but introduces an error for the total velocity
// change impulse (appliedVForce). This could be fixed by having a separate impulse multiplier
// for the velocity change impulse, but the constraint structure would have to be enlarged further.
if (PxAbs(c.appliedForce) > 1e-10f)
c.impulseMultiplier *= c.appliedVForce / c.appliedForce;
// We don't want to over compensate when we remove the impulse that produced the position change
// so we need to remove the over-relaxation on this last iteration.
c.solverExtrapolation = 1.0f;
}
}
bool PxVehicleTireLoadFilterData::isValid() const
{
PX_CHECK_AND_RETURN_VAL(mMaxNormalisedLoad>=mMinNormalisedLoad, "PxVehicleTireLoadFilterData.mMaxNormalisedLoad must be greater than or equal to PxVehicleTireLoadFilterData.mMinNormalisedLoad", false);
PX_CHECK_AND_RETURN_VAL(mMaxFilteredNormalisedLoad>0, "PxVehicleTireLoadFilterData.mMaxFilteredNormalisedLoad must be greater than zero", false);
PX_CHECK_AND_RETURN_VAL(PxAbs((1.0f/(mMaxNormalisedLoad - mMinNormalisedLoad)) - mDenominator) < 0.001f, "PxVehicleTireLoadFilterData.mMaxFilteredNormalisedLoad, PxVehicleTireLoadFilterData.mMinNormalisedLoad, and PxVehicleTireLoadFilterData.mDenominator don't match", false);
return true;
}
void PxVehicleDrive4WSmoothAnalogRawInputsAndSetAnalogInputs
(const PxVehiclePadSmoothingData& padSmoothing, const PxFixedSizeLookupTable<8>& steerVsForwardSpeedTable,
const PxVehicleDrive4WRawInputData& rawInputData,
const PxF32 timestep,
PxVehicleDrive4W& focusVehicle)
{
//gearup/geardown
const bool gearup=rawInputData.getGearUp();
const bool geardown=rawInputData.getGearDown();
focusVehicle.mDriveDynData.setGearUp(gearup);
focusVehicle.mDriveDynData.setGearDown(geardown);
//Update analog inputs for focus vehicle.
//Process the accel.
{
const PxF32 riseRate=padSmoothing.mRiseRates[PxVehicleDrive4W::eANALOG_INPUT_ACCEL];
const PxF32 fallRate=padSmoothing.mFallRates[PxVehicleDrive4W::eANALOG_INPUT_ACCEL];
const PxF32 currentVal=focusVehicle.mDriveDynData.getAnalogInput(PxVehicleDrive4W::eANALOG_INPUT_ACCEL);
const PxF32 targetVal=rawInputData.getAnalogAccel();
const PxF32 accel=processPositiveAnalogValue(riseRate,fallRate,currentVal,targetVal,timestep);
focusVehicle.mDriveDynData.setAnalogInput(accel,PxVehicleDrive4W::eANALOG_INPUT_ACCEL);
}
//Process the brake
{
const PxF32 riseRate=padSmoothing.mRiseRates[PxVehicleDrive4W::eANALOG_INPUT_BRAKE];
const PxF32 fallRate=padSmoothing.mFallRates[PxVehicleDrive4W::eANALOG_INPUT_BRAKE];
const PxF32 currentVal=focusVehicle.mDriveDynData.getAnalogInput(PxVehicleDrive4W::eANALOG_INPUT_BRAKE);
const PxF32 targetVal=rawInputData.getAnalogBrake();
const PxF32 brake=processPositiveAnalogValue(riseRate,fallRate,currentVal,targetVal,timestep);
focusVehicle.mDriveDynData.setAnalogInput(brake,PxVehicleDrive4W::eANALOG_INPUT_BRAKE);
}
//Process the handbrake.
{
const PxF32 riseRate=padSmoothing.mRiseRates[PxVehicleDrive4W::eANALOG_INPUT_HANDBRAKE];
const PxF32 fallRate=padSmoothing.mFallRates[PxVehicleDrive4W::eANALOG_INPUT_HANDBRAKE];
const PxF32 currentVal=focusVehicle.mDriveDynData.getAnalogInput(PxVehicleDrive4W::eANALOG_INPUT_HANDBRAKE);
const PxF32 targetVal=rawInputData.getAnalogHandbrake();
const PxF32 handbrake=processPositiveAnalogValue(riseRate,fallRate,currentVal,targetVal,timestep);
focusVehicle.mDriveDynData.setAnalogInput(handbrake,PxVehicleDrive4W::eANALOG_INPUT_HANDBRAKE);
}
//Process the steer
{
const PxF32 vz=focusVehicle.computeForwardSpeed();
const PxF32 vzAbs=PxAbs(vz);
const bool isInAir=focusVehicle.isInAir();
const PxF32 riseRate=padSmoothing.mRiseRates[PxVehicleDrive4W::eANALOG_INPUT_STEER_RIGHT];
const PxF32 fallRate=padSmoothing.mFallRates[PxVehicleDrive4W::eANALOG_INPUT_STEER_RIGHT];
const PxF32 currentVal=focusVehicle.mDriveDynData.getAnalogInput(PxVehicleDrive4W::eANALOG_INPUT_STEER_RIGHT)-focusVehicle.mDriveDynData.getAnalogInput(PxVehicleDrive4W::eANALOG_INPUT_STEER_LEFT);
const PxF32 targetVal=rawInputData.getAnalogSteer()*(isInAir ? 1.0f :steerVsForwardSpeedTable.getYVal(vzAbs));
const PxF32 steer=processAnalogValue(riseRate,fallRate,currentVal,targetVal,timestep);
focusVehicle.mDriveDynData.setAnalogInput(0.0f, PxVehicleDrive4W::eANALOG_INPUT_STEER_LEFT);
focusVehicle.mDriveDynData.setAnalogInput(steer, PxVehicleDrive4W::eANALOG_INPUT_STEER_RIGHT);
}
}
/**
\brief computes a oriented bounding box around the scaled basis.
\param basis Input = skewed basis, Output = (normalized) orthogonal basis.
\return Bounding box extent.
*/
PxVec3 optimizeBoundingBox(PxMat33& basis)
{
PxVec3* PX_RESTRICT vec = &basis[0]; // PT: don't copy vectors if not needed...
// PT: since we store the magnitudes to memory, we can avoid the FCMPs afterwards
PxVec3 magnitude( vec[0].magnitudeSquared(), vec[1].magnitudeSquared(), vec[2].magnitudeSquared() );
// find indices sorted by magnitude
#ifdef PX_X360
int i = (PxU32&)(magnitude[1]) > (PxU32&)(magnitude[ 0 ]) ? 1 : 0;
int j = (PxU32&)(magnitude[2]) > (PxU32&)(magnitude[1-i]) ? 2 : 1-i;
#else
int i = magnitude[1] > magnitude[ 0 ] ? 1 : 0;
int j = magnitude[2] > magnitude[1-i] ? 2 : 1-i;
#endif
const int k = 3 - i - j;
#ifdef PX_X360
if((PxU32&)(magnitude[i]) < (PxU32&)(magnitude[j]))
#else
if(magnitude[i] < magnitude[j])
#endif
swap(i, j);
// ortho-normalize basis
PxReal invSqrt = PxRecipSqrt(magnitude[i]);
magnitude[i] *= invSqrt; vec[i] *= invSqrt; // normalize the first axis
PxReal dotij = vec[i].dot(vec[j]);
PxReal dotik = vec[i].dot(vec[k]);
magnitude[i] += PxAbs(dotij) + PxAbs(dotik); // elongate the axis by projection of the other two
vec[j] -= vec[i] * dotij; // orthogonize the two remaining axii relative to vec[i]
vec[k] -= vec[i] * dotik;
magnitude[j] = vec[j].normalize();
PxReal dotjk = vec[j].dot(vec[k]);
magnitude[j] += PxAbs(dotjk); // elongate the axis by projection of the other one
vec[k] -= vec[j] * dotjk; // orthogonize vec[k] relative to vec[j]
magnitude[k] = vec[k].normalize();
return magnitude;
}
bool PxVehicleEngineData::isValid() const
{
PX_CHECK_AND_RETURN_VAL(mPeakTorque>0.0f, "PxVehicleEngineData.mPeakTorque must be greater than zero", false);
PX_CHECK_AND_RETURN_VAL(mMaxOmega>0.0f, "PxVehicleEngineData.mMaxOmega must be greater than zero", false);
PX_CHECK_AND_RETURN_VAL(mDampingRateFullThrottle>0.0f, "PxVehicleEngineData.mDampingRateFullThrottle must be greater than zero", false);
PX_CHECK_AND_RETURN_VAL(mDampingRateZeroThrottleClutchEngaged>0.0f, "PxVehicleEngineData.mDampingRateZeroThrottleClutchEngaged must be greater than zero", false);
PX_CHECK_AND_RETURN_VAL(mDampingRateZeroThrottleClutchDisengaged>0.0f, "PxVehicleEngineData.mDampingRateZeroThrottleClutchDisengaged must be greater than zero", false);
PX_CHECK_AND_RETURN_VAL(mRecipMaxOmega>0.0f, "PxVehicleEngineData.mRecipMaxOmega must be greater than zero", false);
PX_CHECK_AND_RETURN_VAL(PxAbs((1.0f/mMaxOmega)-mRecipMaxOmega) <= 0.001f, "PxVehicleEngineData.mMaxOmega and PxVehicleEngineData.mRecipMaxOmega don't match", false);
return true;
}
//process value in range(-1,1)
PX_FORCE_INLINE PxF32 processAnalogValue
(const PxF32 riseRate, const PxF32 fallRate,
const PxF32 currentVal, const PxF32 targetVal,
const PxF32 timestep)
{
PX_ASSERT(PxAbs(targetVal)<=1.01f);
PxF32 val=0.0f; // PT: the following code could leave that variable uninitialized!!!!!
if(0==targetVal)
{
//Drift slowly back to zero
if(currentVal>0)
{
val=currentVal-fallRate*timestep;
val=PxMax(val,0.0f);
}
else if(currentVal<0)
{
val=currentVal+fallRate*timestep;
val=PxMin(val,0.0f);
}
}
else
{
if(currentVal < targetVal)
{
if(currentVal<0)
{
val=currentVal + fallRate*timestep;
val=PxMin(val,targetVal);
}
else
{
val=currentVal + riseRate*timestep;
val=PxMin(val,targetVal);
}
}
else
{
if(currentVal>0)
{
val=currentVal - fallRate*timestep;
val=PxMax(val,targetVal);
}
else
{
val=currentVal - riseRate*timestep;
val=PxMax(val,targetVal);
}
}
}
return val;
}
bool PxVehicleWheels4SimData::isValid(const PxU32 id) const
{
PX_ASSERT(id<4);
PX_CHECK_AND_RETURN_VAL(mSuspensions[id].isValid(), "Invalid PxVehicleSuspWheelTire4SimulationData.mSuspensions", false);
PX_CHECK_AND_RETURN_VAL(mWheels[id].isValid(), "Invalid PxVehicleSuspWheelTire4SimulationData.mWheels", false);
PX_CHECK_AND_RETURN_VAL(mTires[id].isValid(), "Invalid PxVehicleSuspWheelTire4SimulationData.mTires", false);
PX_CHECK_AND_RETURN_VAL(mSuspDownwardTravelDirections[id].magnitude()>=0.999f && mSuspDownwardTravelDirections[id].magnitude()<=1.001f, "Invalid PxVehicleSuspWheelTire4SimulationData.mSuspDownwardTravelDirections", false);
PX_CHECK_AND_RETURN_VAL(mSuspForceAppPointOffsets[id].magnitude()!=0.0f, "Invalid PxVehicleSuspWheelTire4SimulationData.mSuspForceAppPointOffsets.mSuspForceAppPointOffsets", false);
PX_CHECK_AND_RETURN_VAL(mTireForceAppPointOffsets[id].magnitude()!=0.0f, "Invalid PxVehicleSuspWheelTire4SimulationData.mTireForceAppPointOffsets.mTireForceAppPointOffsets", false);
PX_CHECK_AND_RETURN_VAL(mWheelCentreOffsets[id].magnitude()!=0.0f, "Invalid PxVehicleSuspWheelTire4SimulationData.mWheelCentreOffsets.mWheelCentreOffsets", false);
PX_CHECK_AND_RETURN_VAL(mTireRestLoads[id]>0.0f, "Invalid PxVehicleSuspWheelTire4SimulationData.mTireRestLoads", false);
PX_CHECK_AND_RETURN_VAL(PxAbs((1.0f/mTireRestLoads[id]) - mRecipTireRestLoads[id]) <= 0.001f, "Invalid PxVehicleSuspWheelTire4SimulationData.mRecipTireRestLoads", false);
PX_UNUSED(id);
return true;
}
void PxVehicleDrive4WSmoothDigitalRawInputsAndSetAnalogInputs
(const PxVehicleKeySmoothingData& keySmoothing, const PxFixedSizeLookupTable<8>& steerVsForwardSpeedTable,
const PxVehicleDrive4WRawInputData& rawInputData,
const PxF32 timestep,
PxVehicleDrive4W& focusVehicle)
{
const bool gearup=rawInputData.getGearUp();
const bool geardown=rawInputData.getGearDown();
focusVehicle.mDriveDynData.setGearDown(geardown);
focusVehicle.mDriveDynData.setGearUp(gearup);
const PxF32 accel=processDigitalValue(PxVehicleDrive4W::eANALOG_INPUT_ACCEL,keySmoothing,rawInputData.getDigitalAccel(),timestep,focusVehicle.mDriveDynData.getAnalogInput(PxVehicleDrive4W::eANALOG_INPUT_ACCEL));
focusVehicle.mDriveDynData.setAnalogInput(accel,PxVehicleDrive4W::eANALOG_INPUT_ACCEL);
const PxF32 brake=processDigitalValue(PxVehicleDrive4W::eANALOG_INPUT_BRAKE,keySmoothing,rawInputData.getDigitalBrake(),timestep,focusVehicle.mDriveDynData.getAnalogInput(PxVehicleDrive4W::eANALOG_INPUT_BRAKE));
focusVehicle.mDriveDynData.setAnalogInput(brake,PxVehicleDrive4W::eANALOG_INPUT_BRAKE);
const PxF32 handbrake=processDigitalValue(PxVehicleDrive4W::eANALOG_INPUT_HANDBRAKE,keySmoothing,rawInputData.getDigitalHandbrake(),timestep,focusVehicle.mDriveDynData.getAnalogInput(PxVehicleDrive4W::eANALOG_INPUT_HANDBRAKE));
focusVehicle.mDriveDynData.setAnalogInput(handbrake,PxVehicleDrive4W::eANALOG_INPUT_HANDBRAKE);
PxF32 steerLeft=processDigitalValue(PxVehicleDrive4W::eANALOG_INPUT_STEER_LEFT,keySmoothing,rawInputData.getDigitalSteerLeft(),timestep,focusVehicle.mDriveDynData.getAnalogInput(PxVehicleDrive4W::eANALOG_INPUT_STEER_LEFT));
PxF32 steerRight=processDigitalValue(PxVehicleDrive4W::eANALOG_INPUT_STEER_RIGHT,keySmoothing,rawInputData.getDigitalSteerRight(),timestep,focusVehicle.mDriveDynData.getAnalogInput(PxVehicleDrive4W::eANALOG_INPUT_STEER_RIGHT));
const PxF32 vz=focusVehicle.computeForwardSpeed();
const PxF32 vzAbs=PxAbs(vz);
const bool isInAir=focusVehicle.isInAir();
const PxF32 maxSteer=(isInAir ? 1.0f :steerVsForwardSpeedTable.getYVal(vzAbs));
const PxF32 steer=PxAbs(steerRight-steerLeft);
if(steer>maxSteer)
{
const PxF32 k=maxSteer/steer;
steerLeft*=k;
steerRight*=k;
}
focusVehicle.mDriveDynData.setAnalogInput(steerLeft, PxVehicleDrive4W::eANALOG_INPUT_STEER_LEFT);
focusVehicle.mDriveDynData.setAnalogInput(steerRight, PxVehicleDrive4W::eANALOG_INPUT_STEER_RIGHT);
}
PX_INLINE void computeFrictionTangents(const PxVec3& vrel,const PxVec3& unitNormal, PxVec3& t0, PxVec3& t1)
{
PX_ASSERT(PxAbs(unitNormal.magnitude()-1)<1e-3f);
t0 = vrel - unitNormal * unitNormal.dot(vrel);
PxReal ll = t0.magnitudeSquared();
if (ll > 0.1f) //can set as low as 0.
{
t0 *= PxRecipSqrt(ll);
t1 = unitNormal.cross(t0);
}
else
Ps::normalToTangents(unitNormal, t0, t1); //fallback
}
void PxVehicleWheels4SimData::setWheelData(const PxU32 id, const PxVehicleWheelData& wheel)
{
PX_CHECK_AND_RETURN(id<4, "Illegal wheel id");
PX_CHECK_AND_RETURN(wheel.mRadius>0, "Wheel radius must be greater than zero");
PX_CHECK_AND_RETURN(wheel.mMaxBrakeTorque>=0, "Wheel brake torque must be zero or be a positive value");
PX_CHECK_AND_RETURN(wheel.mMaxHandBrakeTorque>=0, "Wheel handbrake torque must be zero or be a positive value");
PX_CHECK_AND_RETURN(PxAbs(wheel.mMaxSteer)<PxHalfPi, "Wheel max steer must be in range (-Pi/2,Pi/2)");
PX_CHECK_AND_RETURN(wheel.mMass>0, "Wheel mass must be greater than zero");
PX_CHECK_AND_RETURN(wheel.mMOI>0, "Wheel moi must be greater than zero");
PX_CHECK_AND_RETURN(wheel.mToeAngle>-PxHalfPi && wheel.mToeAngle<PxHalfPi, "Wheel toe angle must be in range (-Pi/2,Pi/2)");
PX_CHECK_AND_RETURN(wheel.mWidth>0, "Wheel width must be greater than zero");
PX_CHECK_AND_RETURN(wheel.mDampingRate>=0, "Wheel damping rate must be greater than or equal to zero");
mWheels[id]=wheel;
mWheels[id].mRecipRadius=1.0f/mWheels[id].mRadius;
mWheels[id].mRecipMOI=1.0f/mWheels[id].mMOI;
mTireRestLoads[id]=mWheels[id].mMass+mSuspensions[id].mSprungMass;
mRecipTireRestLoads[id]=1.0f/mTireRestLoads[id];
}
bool setupFinalizeExtSolverConstraintsCoulomb(PxcNpWorkUnit& n,
const ContactBuffer& buffer,
const PxcCorrelationBufferCoulomb& c,
const PxTransform& bodyFrame0,
const PxTransform& bodyFrame1,
bool /*perPointFriction*/,
PxU8* workspace,
PxReal invDt,
PxReal bounceThreshold,
PxsSolverExtBody& b0,
PxsSolverExtBody& b1,
PxU32 frictionCountPerPoint,
PxReal invMassScale0, PxReal invInertiaScale0,
PxReal invMassScale1, PxReal invInertiaScale1)
{
// NOTE II: the friction patches are sparse (some of them have no contact patches, and
// therefore did not get written back to the cache) but the patch addresses are dense,
// corresponding to valid patches
PxU8* PX_RESTRICT ptr = workspace;
const FloatV zero=FZero();
//KS - TODO - this should all be done in SIMD to avoid LHS
const PxF32 maxPenBias0 = b0.mLinkIndex == PxcSolverConstraintDesc::NO_LINK ? b0.mBodyData->penBiasClamp : getMaxPenBias(*b0.mFsData)[b0.mLinkIndex];
const PxF32 maxPenBias1 = b1.mLinkIndex == PxcSolverConstraintDesc::NO_LINK ? b1.mBodyData->penBiasClamp : getMaxPenBias(*b1.mFsData)[b0.mLinkIndex];
const FloatV maxPen = FLoad(PxMax(maxPenBias0, maxPenBias1)/invDt);
const FloatV restDistance = FLoad(n.restDistance);
Ps::prefetchLine(c.contactID);
Ps::prefetchLine(c.contactID, 128);
bool useExtContacts = (n.flags & (PxcNpWorkUnitFlag::eARTICULATION_BODY0|PxcNpWorkUnitFlag::eARTICULATION_BODY1))!=0;
const PxU32 frictionPatchCount = c.frictionPatchCount;
const bool staticBody = ((n.flags & PxcNpWorkUnitFlag::eDYNAMIC_BODY1) == 0);
const PxU32 pointStride = useExtContacts ? sizeof(PxcSolverContactExt) : sizeof(PxcSolverContact);
const PxU32 frictionStride = useExtContacts ? sizeof(PxcSolverFrictionExt) : sizeof(PxcSolverFriction);
const PxU8 pointHeaderType = Ps::to8(useExtContacts ? PXS_SC_TYPE_EXT_CONTACT : (staticBody ? PXS_SC_TYPE_STATIC_CONTACT : PXS_SC_TYPE_RB_CONTACT));
const PxU8 frictionHeaderType = Ps::to8(useExtContacts ? PXS_SC_TYPE_EXT_FRICTION : (staticBody ? PXS_SC_TYPE_STATIC_FRICTION : PXS_SC_TYPE_FRICTION));
PxReal d0 = n.dominance0 * invMassScale0;
PxReal d1 = n.dominance1 * invMassScale1;
PxReal angD0 = n.dominance0 * invInertiaScale0;
PxReal angD1 = n.dominance1 * invInertiaScale1;
for(PxU32 i=0;i< frictionPatchCount;i++)
{
const PxU32 contactCount = c.frictionPatchContactCounts[i];
if(contactCount == 0)
continue;
const Gu::ContactPoint* contactBase0 = buffer.contacts + c.contactPatches[c.correlationListHeads[i]].start;
const PxcFrictionPatchCoulomb& frictionPatch = c.frictionPatches[i];
const Vec3V normalV = Ps::aos::V3LoadU(frictionPatch.normal);
const PxVec3 normal = frictionPatch.normal;
const PxReal combinedRestitution = contactBase0->restitution;
PxcSolverContactCoulombHeader* PX_RESTRICT header = reinterpret_cast<PxcSolverContactCoulombHeader*>(ptr);
ptr += sizeof(PxcSolverContactCoulombHeader);
Ps::prefetchLine(ptr, 128);
Ps::prefetchLine(ptr, 256);
Ps::prefetchLine(ptr, 384);
header->numNormalConstr = (PxU8)contactCount;
header->type = pointHeaderType;
header->setRestitution(combinedRestitution);
header->setDominance0(d0);
header->setDominance1(d1);
header->angDom0 = angD0;
header->angDom1 = angD1;
header->setNormal(normalV);
for(PxU32 patch=c.correlationListHeads[i];
patch!=PxcCorrelationBuffer::LIST_END;
patch = c.contactPatches[patch].next)
{
const PxU32 count = c.contactPatches[patch].count;
const Gu::ContactPoint* contactBase = buffer.contacts + c.contactPatches[patch].start;
PxU8* p = ptr;
for(PxU32 j=0;j<count;j++)
{
const Gu::ContactPoint& contact = contactBase[j];
PxcSolverContactExt* PX_RESTRICT solverContact = reinterpret_cast<PxcSolverContactExt*>(p);
p += pointStride;
const FloatV separation = FLoad(contact.separation);
PxVec3 ra = contact.point - bodyFrame0.p;
PxVec3 rb = contact.point - bodyFrame1.p;
Vec3V targetVel = V3LoadU(contact.targetVel);
const FloatV maxImpulse = FLoad(contact.maxImpulse);
solverContact->scaledBiasX_targetVelocityY_maxImpulseZ = V3Merge(FMax(maxPen, FSub(separation, restDistance)), V3Dot(normalV,targetVel), maxImpulse);
//TODO - should we do cross only in vector land and then store. Could cause a LHS but probably no worse than
//what we already have (probably has a LHS from converting from vector to scalar above)
const PxVec3 raXn = ra.cross(normal);
const PxVec3 rbXn = rb.cross(normal);
Cm::SpatialVector deltaV0, deltaV1;
PxReal unitResponse = getImpulseResponse(b0, Cm::SpatialVector(normal, raXn), deltaV0, d0, angD0,
b1, Cm::SpatialVector(-normal, -rbXn), deltaV1, d1, angD1);
const PxReal vrel = b0.projectVelocity(normal, raXn)
- b1.projectVelocity(normal, rbXn);
solverContact->raXnXYZ_appliedForceW = V4SetW(Vec4V_From_Vec3V(V3LoadU(raXn)), zero);
solverContact->rbXnXYZ_velMultiplierW = V4SetW(Vec4V_From_Vec3V(V3LoadU(rbXn)), zero);
completeContactPoint(*solverContact, unitResponse, vrel, invDt, header->restitution, bounceThreshold);
solverContact->setDeltaVA(deltaV0.linear, deltaV0.angular);
solverContact->setDeltaVB(deltaV1.linear, deltaV1.angular);
}
ptr = p;
}
}
//construct all the frictions
PxU8* PX_RESTRICT ptr2 = workspace;
const PxF32 orthoThreshold = 0.70710678f;
const PxF32 eps = 0.00001f;
bool hasFriction = false;
for(PxU32 i=0;i< frictionPatchCount;i++)
{
const PxU32 contactCount = c.frictionPatchContactCounts[i];
if(contactCount == 0)
continue;
PxcSolverContactCoulombHeader* header = reinterpret_cast<PxcSolverContactCoulombHeader*>(ptr2);
header->frictionOffset = PxU16(ptr - ptr2);
ptr2 += sizeof(PxcSolverContactCoulombHeader) + header->numNormalConstr * pointStride;
PxVec3 normal = c.frictionPatches[i].normal;
const Gu::ContactPoint* contactBase0 = buffer.contacts + c.contactPatches[c.correlationListHeads[i]].start;
const PxReal staticFriction = contactBase0->staticFriction;
const PxU32 disableStrongFriction = contactBase0->internalFaceIndex1 & PxMaterialFlag::eDISABLE_FRICTION;
const bool haveFriction = (disableStrongFriction == 0);
PxcSolverFrictionHeader* frictionHeader = (PxcSolverFrictionHeader*)ptr;
frictionHeader->numNormalConstr = Ps::to8(c.frictionPatchContactCounts[i]);
frictionHeader->numFrictionConstr = Ps::to8(haveFriction ? c.frictionPatches[i].numConstraints : 0);
ptr += sizeof(PxcSolverFrictionHeader);
ptr += frictionHeader->getAppliedForcePaddingSize(c.frictionPatchContactCounts[i]);
Ps::prefetchLine(ptr, 128);
Ps::prefetchLine(ptr, 256);
Ps::prefetchLine(ptr, 384);
const PxVec3 t0Fallback1(0.f, -normal.z, normal.y);
const PxVec3 t0Fallback2(-normal.y, normal.x, 0.f) ;
const PxVec3 tFallback1 = orthoThreshold > PxAbs(normal.x) ? t0Fallback1 : t0Fallback2;
const PxVec3 vrel = b0.getLinVel() - b1.getLinVel();
const PxVec3 t0_ = vrel - normal * (normal.dot(vrel));
const PxReal sqDist = t0_.dot(t0_);
const PxVec3 tDir0 = (sqDist > eps ? t0_: tFallback1).getNormalized();
const PxVec3 tDir1 = tDir0.cross(normal);
PxVec3 tFallback[2] = {tDir0, tDir1};
PxU32 ind = 0;
if(haveFriction)
{
hasFriction = true;
frictionHeader->setStaticFriction(staticFriction);
frictionHeader->setDominance0(n.dominance0);
frictionHeader->setDominance1(n.dominance1);
frictionHeader->angDom0 = angD0;
frictionHeader->angDom1 = angD1;
frictionHeader->type = frictionHeaderType;
PxU32 totalPatchContactCount = 0;
for(PxU32 patch=c.correlationListHeads[i];
patch!=PxcCorrelationBuffer::LIST_END;
patch = c.contactPatches[patch].next)
{
const PxU32 count = c.contactPatches[patch].count;
const PxU32 start = c.contactPatches[patch].start;
const Gu::ContactPoint* contactBase = buffer.contacts + start;
PxU8* p = ptr;
for(PxU32 j =0; j < count; j++)
{
const PxU32 contactId = totalPatchContactCount + j;
const Gu::ContactPoint& contact = contactBase[j];
const PxVec3 ra = contact.point - bodyFrame0.p;
const PxVec3 rb = contact.point - bodyFrame1.p;
for(PxU32 k = 0; k < frictionCountPerPoint; ++k)
{
PxcSolverFrictionExt* PX_RESTRICT f0 = reinterpret_cast<PxcSolverFrictionExt*>(p);
p += frictionStride;
f0->contactIndex = contactId;
PxVec3 t0 = tFallback[ind];
ind = 1 - ind;
PxVec3 raXn = ra.cross(t0);
PxVec3 rbXn = rb.cross(t0);
Cm::SpatialVector deltaV0, deltaV1;
PxReal unitResponse = getImpulseResponse(b0, Cm::SpatialVector(t0, raXn), deltaV0, d0, angD0,
b1, Cm::SpatialVector(-t0, -rbXn), deltaV1, d1, angD1);
f0->setVelMultiplier(FLoad(unitResponse>0.0f ? 1.f/unitResponse : 0.0f));
f0->setRaXn(raXn);
f0->setRbXn(rbXn);
f0->setNormal(t0);
f0->setAppliedForce(0.0f);
f0->setDeltaVA(deltaV0.linear, deltaV0.angular);
f0->setDeltaVB(deltaV1.linear, deltaV1.angular);
}
}
totalPatchContactCount += c.contactPatches[patch].count;
ptr = p;
}
}
}
//PX_ASSERT(ptr - workspace == n.solverConstraintSize);
return hasFriction;
}
void PxVehicleWheels::setup
(PxPhysics* physics, PxRigidDynamic* vehActor,
const PxVehicleWheelsSimData& wheelsData,
const PxU32 numDrivenWheels, const PxU32 numNonDrivenWheels)
{
PX_CHECK_AND_RETURN(wheelsData.getNumWheels() == mWheelsSimData.getNumWheels(), "PxVehicleWheels::setup - vehicle must be setup with same number of wheels as wheelsData");
PX_CHECK_AND_RETURN(vehActor, "PxVehicleWheels::setup - vehActor is null ptr : you need to instantiate an empty PxRigidDynamic for the vehicle");
PX_CHECK_AND_RETURN(vehActor->getNbShapes() >= (numDrivenWheels + numNonDrivenWheels + 1), "PxVehicleWheels::setup - not enough wheels shapes have been added to vehActor");
PX_CHECK_AND_RETURN(wheelsData.isValid(), "PxVehicleWheels::setup -invalid wheelsData");
#ifdef PX_CHECKED
PxF32 totalSprungMass=0.0f;
for(PxU32 i=0;i<(numDrivenWheels+numNonDrivenWheels);i++)
{
totalSprungMass+=wheelsData.getSuspensionData(i).mSprungMass;
}
PX_CHECK_MSG(PxAbs((vehActor->getMass()-totalSprungMass)/vehActor->getMass()) < 0.01f, "Sum of suspension sprung masses doesn't match actor mass");
#endif
//Copy the simulation data.
mWheelsSimData=wheelsData;
//Set the actor pointer.
mActor=vehActor;
//Pose the wheels from the wheels offset data.
const PxTransform chassisCMOffset=vehActor->getCMassLocalPose();
for(PxU32 i=0;i<wheelsData.mNumActiveWheels;i++)
{
if(mWheelShapeMap[i]!=PX_MAX_U8)
{
//Get the shape.
const PxU32 shapeIndex=mWheelShapeMap[i];
PxShape* shapeBuffer[1];
vehActor->getShapes(shapeBuffer,1,shapeIndex);
PxTransform wheelOffset=chassisCMOffset;
wheelOffset.p+=wheelsData.getWheelCentreOffset(i);
shapeBuffer[0]->setLocalPose(wheelOffset);
}
}
//Set all the sq result ptrs to null.
const PxU32 numSuspWheelTire4=wheelsData.mNumWheels4;
for(PxU32 i=0;i<numSuspWheelTire4;i++)
{
mWheelsDynData.mWheels4DynData[i].mSqResults=NULL;
}
//Set up the suspension limits constraints.
for(PxU32 i=0;i<numSuspWheelTire4;i++)
{
PxVehicleConstraintShader& shader=mWheelsDynData.mWheels4DynData[i].getVehicletConstraintShader();
for(PxU32 j=0;j<4;j++)
{
shader.mData.mSuspLimitData.mCMOffsets[j]=wheelsData.mWheels4SimData[i].getSuspForceAppPointOffset(j);
shader.mData.mSuspLimitData.mDirs[j]=wheelsData.mWheels4SimData[i].getSuspTravelDirection(j);
shader.mData.mSuspLimitData.mErrors[j]=0.0f;
shader.mData.mSuspLimitData.mActiveFlags[j]=false;
shader.mData.mStickyTireData.mCMOffsets[j]=PxVec3(0,0,0);
shader.mData.mStickyTireData.mDirs[j]=PxVec3(0,0,0);
shader.mData.mStickyTireData.mTargetSpeeds[j]=0.0f;
shader.mData.mStickyTireData.mActiveFlags[j]=false;
}
#ifdef PX_PS3
PxConstraintShaderTable t =
{
PxVehicleConstraintShader::vehicleSuspLimitConstraintSolverPrep,
PxVehicle4WSuspLimitConstraintShaderSpu,
PXVEHICLE4WSUSPLIMITCONSTRAINTSHADERSPU_SIZE,
0,
PxVehicleConstraintShader::visualiseConstraint
};
#else
PxConstraintShaderTable t =
{
PxVehicleConstraintShader::vehicleSuspLimitConstraintSolverPrep,
0,
0,
0,
PxVehicleConstraintShader::visualiseConstraint
};
#endif
shader.mConstraint=physics->createConstraint(vehActor, NULL, shader, t, sizeof(PxVehicleConstraintShader::VehicleConstraintData));
shader.mConstraint->markDirty();
}
//Set up the shader data ptrs.
for(PxU32 i=0;i<wheelsData.mNumActiveWheels;i++)
{
mWheelsDynData.setTireForceShaderData(i,&mWheelsSimData.getTireData(i));
}
//Disable the unused wheels.
for(PxU32 i=wheelsData.mNumActiveWheels;i<4*mWheelsSimData.mNumWheels4;i++)
{
mWheelsSimData.disableWheel(i);
}
}
bool setupFinalizeExtSolverContactsCoulomb(
const ContactBuffer& buffer,
const CorrelationBuffer& c,
const PxTransform& bodyFrame0,
const PxTransform& bodyFrame1,
PxU8* workspace,
PxReal invDt,
PxReal bounceThresholdF32,
const SolverExtBody& b0,
const SolverExtBody& b1,
PxU32 frictionCountPerPoint,
PxReal invMassScale0, PxReal invInertiaScale0,
PxReal invMassScale1, PxReal invInertiaScale1,
PxReal restDist,
PxReal ccdMaxDistance)
{
// NOTE II: the friction patches are sparse (some of them have no contact patches, and
// therefore did not get written back to the cache) but the patch addresses are dense,
// corresponding to valid patches
const FloatV ccdMaxSeparation = FLoad(ccdMaxDistance);
PxU8* PX_RESTRICT ptr = workspace;
//KS - TODO - this should all be done in SIMD to avoid LHS
const PxF32 maxPenBias0 = b0.mLinkIndex == PxSolverConstraintDesc::NO_LINK ? b0.mBodyData->penBiasClamp : getMaxPenBias(*b0.mFsData)[b0.mLinkIndex];
const PxF32 maxPenBias1 = b1.mLinkIndex == PxSolverConstraintDesc::NO_LINK ? b1.mBodyData->penBiasClamp : getMaxPenBias(*b1.mFsData)[b1.mLinkIndex];
const FloatV maxPenBias = FLoad(PxMax(maxPenBias0, maxPenBias1)/invDt);
const FloatV restDistance = FLoad(restDist);
const FloatV bounceThreshold = FLoad(bounceThresholdF32);
const FloatV invDtV = FLoad(invDt);
const FloatV pt8 = FLoad(0.8f);
const FloatV invDtp8 = FMul(invDtV, pt8);
Ps::prefetchLine(c.contactID);
Ps::prefetchLine(c.contactID, 128);
const PxU32 frictionPatchCount = c.frictionPatchCount;
const PxU32 pointStride = sizeof(SolverContactPointExt);
const PxU32 frictionStride = sizeof(SolverContactFrictionExt);
const PxU8 pointHeaderType = DY_SC_TYPE_EXT_CONTACT;
const PxU8 frictionHeaderType = DY_SC_TYPE_EXT_FRICTION;
PxReal d0 = invMassScale0;
PxReal d1 = invMassScale1;
PxReal angD0 = invInertiaScale0;
PxReal angD1 = invInertiaScale1;
PxU8 flags = 0;
for(PxU32 i=0;i< frictionPatchCount;i++)
{
const PxU32 contactCount = c.frictionPatchContactCounts[i];
if(contactCount == 0)
continue;
const Gu::ContactPoint* contactBase0 = buffer.contacts + c.contactPatches[c.correlationListHeads[i]].start;
const Vec3V normalV = Ps::aos::V3LoadA(contactBase0->normal);
const Vec3V normal = V3LoadA(contactBase0->normal);
const PxReal combinedRestitution = contactBase0->restitution;
SolverContactCoulombHeader* PX_RESTRICT header = reinterpret_cast<SolverContactCoulombHeader*>(ptr);
ptr += sizeof(SolverContactCoulombHeader);
Ps::prefetchLine(ptr, 128);
Ps::prefetchLine(ptr, 256);
Ps::prefetchLine(ptr, 384);
const FloatV restitution = FLoad(combinedRestitution);
header->numNormalConstr = PxU8(contactCount);
header->type = pointHeaderType;
//header->setRestitution(combinedRestitution);
header->setDominance0(d0);
header->setDominance1(d1);
header->angDom0 = angD0;
header->angDom1 = angD1;
header->flags = flags;
header->setNormal(normalV);
for(PxU32 patch=c.correlationListHeads[i];
patch!=CorrelationBuffer::LIST_END;
patch = c.contactPatches[patch].next)
{
const PxU32 count = c.contactPatches[patch].count;
const Gu::ContactPoint* contactBase = buffer.contacts + c.contactPatches[patch].start;
PxU8* p = ptr;
for(PxU32 j=0;j<count;j++)
{
const Gu::ContactPoint& contact = contactBase[j];
SolverContactPointExt* PX_RESTRICT solverContact = reinterpret_cast<SolverContactPointExt*>(p);
p += pointStride;
setupExtSolverContact(b0, b1, d0, d1, angD0, angD1, bodyFrame0, bodyFrame1, normal, invDtV, invDtp8, restDistance, maxPenBias, restitution,
bounceThreshold, contact, *solverContact, ccdMaxSeparation);
}
ptr = p;
}
}
//construct all the frictions
PxU8* PX_RESTRICT ptr2 = workspace;
const PxF32 orthoThreshold = 0.70710678f;
const PxF32 eps = 0.00001f;
bool hasFriction = false;
for(PxU32 i=0;i< frictionPatchCount;i++)
{
const PxU32 contactCount = c.frictionPatchContactCounts[i];
if(contactCount == 0)
continue;
SolverContactCoulombHeader* header = reinterpret_cast<SolverContactCoulombHeader*>(ptr2);
header->frictionOffset = PxU16(ptr - ptr2);
ptr2 += sizeof(SolverContactCoulombHeader) + header->numNormalConstr * pointStride;
const Gu::ContactPoint* contactBase0 = buffer.contacts + c.contactPatches[c.correlationListHeads[i]].start;
PxVec3 normal = contactBase0->normal;
const PxReal staticFriction = contactBase0->staticFriction;
const bool disableStrongFriction = !!(contactBase0->materialFlags & PxMaterialFlag::eDISABLE_FRICTION);
const bool haveFriction = (disableStrongFriction == 0);
SolverFrictionHeader* frictionHeader = reinterpret_cast<SolverFrictionHeader*>(ptr);
frictionHeader->numNormalConstr = Ps::to8(c.frictionPatchContactCounts[i]);
frictionHeader->numFrictionConstr = Ps::to8(haveFriction ? c.frictionPatchContactCounts[i] * frictionCountPerPoint : 0);
frictionHeader->flags = flags;
ptr += sizeof(SolverFrictionHeader);
PxF32* forceBuffer = reinterpret_cast<PxF32*>(ptr);
ptr += frictionHeader->getAppliedForcePaddingSize(c.frictionPatchContactCounts[i]);
PxMemZero(forceBuffer, sizeof(PxF32) * c.frictionPatchContactCounts[i]);
Ps::prefetchLine(ptr, 128);
Ps::prefetchLine(ptr, 256);
Ps::prefetchLine(ptr, 384);
const PxVec3 t0Fallback1(0.f, -normal.z, normal.y);
const PxVec3 t0Fallback2(-normal.y, normal.x, 0.f) ;
const PxVec3 tFallback1 = orthoThreshold > PxAbs(normal.x) ? t0Fallback1 : t0Fallback2;
const PxVec3 vrel = b0.getLinVel() - b1.getLinVel();
const PxVec3 t0_ = vrel - normal * (normal.dot(vrel));
const PxReal sqDist = t0_.dot(t0_);
const PxVec3 tDir0 = (sqDist > eps ? t0_: tFallback1).getNormalized();
const PxVec3 tDir1 = tDir0.cross(normal);
PxVec3 tFallback[2] = {tDir0, tDir1};
PxU32 ind = 0;
if(haveFriction)
{
hasFriction = true;
frictionHeader->setStaticFriction(staticFriction);
frictionHeader->invMass0D0 = d0;
frictionHeader->invMass1D1 = d1;
frictionHeader->angDom0 = angD0;
frictionHeader->angDom1 = angD1;
frictionHeader->type = frictionHeaderType;
PxU32 totalPatchContactCount = 0;
for(PxU32 patch=c.correlationListHeads[i];
patch!=CorrelationBuffer::LIST_END;
patch = c.contactPatches[patch].next)
{
const PxU32 count = c.contactPatches[patch].count;
const PxU32 start = c.contactPatches[patch].start;
const Gu::ContactPoint* contactBase = buffer.contacts + start;
PxU8* p = ptr;
for(PxU32 j =0; j < count; j++)
{
const Gu::ContactPoint& contact = contactBase[j];
const PxVec3 ra = contact.point - bodyFrame0.p;
const PxVec3 rb = contact.point - bodyFrame1.p;
const PxVec3 targetVel = contact.targetVel;
const PxVec3 pVRa = b0.getLinVel() + b0.getAngVel().cross(ra);
const PxVec3 pVRb = b1.getLinVel() + b1.getAngVel().cross(rb);
//const PxVec3 vrel = pVRa - pVRb;
for(PxU32 k = 0; k < frictionCountPerPoint; ++k)
{
SolverContactFrictionExt* PX_RESTRICT f0 = reinterpret_cast<SolverContactFrictionExt*>(p);
p += frictionStride;
PxVec3 t0 = tFallback[ind];
ind = 1 - ind;
PxVec3 raXn = ra.cross(t0);
PxVec3 rbXn = rb.cross(t0);
Cm::SpatialVector deltaV0, deltaV1;
const Cm::SpatialVector resp0 = createImpulseResponseVector(t0, raXn, b0);
const Cm::SpatialVector resp1 = createImpulseResponseVector(-t0, -rbXn, b1);
PxReal unitResponse = getImpulseResponse(b0, resp0, deltaV0, d0, angD0,
b1, resp1, deltaV1, d1, angD1);
PxReal tv = targetVel.dot(t0);
if(b0.mLinkIndex == PxSolverConstraintDesc::NO_LINK)
tv += pVRa.dot(t0);
else if(b1.mLinkIndex == PxSolverConstraintDesc::NO_LINK)
tv -= pVRb.dot(t0);
f0->setVelMultiplier(FLoad(unitResponse>0.0f ? 1.f/unitResponse : 0.0f));
f0->setRaXn(resp0.angular);
f0->setRbXn(-resp1.angular);
f0->targetVel = tv;
f0->setNormal(t0);
f0->setAppliedForce(0.0f);
f0->linDeltaVA = V3LoadA(deltaV0.linear);
f0->angDeltaVA = V3LoadA(deltaV0.angular);
f0->linDeltaVB = V3LoadA(deltaV1.linear);
f0->angDeltaVB = V3LoadA(deltaV1.angular);
}
}
totalPatchContactCount += c.contactPatches[patch].count;
ptr = p;
}
}
}
//PX_ASSERT(ptr - workspace == n.solverConstraintSize);
return hasFriction;
}
bool PxcContactCapsuleCapsule(CONTACT_METHOD_ARGS)
{
PX_UNUSED(npCache);
// Get actual shape data
const PxCapsuleGeometry& shapeCapsule0 = shape0.get<const PxCapsuleGeometry>();
const PxCapsuleGeometry& shapeCapsule1 = shape1.get<const PxCapsuleGeometry>();
// Capsule-capsule contact generation
Gu::Segment segment[2];
segment[0].p0 = transform0.q.getBasisVector0() * shapeCapsule0.halfHeight;
segment[0].p1 = -segment[0].p0;
segment[1].p0 = transform1.q.getBasisVector0() * shapeCapsule1.halfHeight;
segment[1].p1 = -segment[1].p0;
PxVec3 delta = transform1.p - transform0.p;
segment[1].p1 += delta;
segment[1].p0 += delta;
// Collision detection
PxReal s,t;
#ifdef USE_NEW_VERSION
PxReal squareDist = Gu::distanceSegmentSegmentSquared2(segment[0], segment[1], &s, &t);
#else
PxReal squareDist = PxcDistanceSegmentSegmentSquaredOLD(segment[0].point0, segment[0].direction(), segment[1].point0, segment[1].direction(), &s, &t);
#endif
const PxReal radiusSum = shapeCapsule0.radius + shapeCapsule1.radius;
const PxReal inflatedSum = radiusSum + contactDistance;
const PxReal inflatedSumSquared = inflatedSum*inflatedSum;
if(squareDist < inflatedSumSquared)
{
PxVec3 dir[2];
dir[0] = segment[0].computeDirection();
dir[1] = segment[1].computeDirection();
PxReal segLen[2];
segLen[0] = dir[0].magnitude();
segLen[1] = dir[1].magnitude();
if (segLen[0]) dir[0] *= 1.0f / segLen[0];
if (segLen[1]) dir[1] *= 1.0f / segLen[1];
if (PxAbs(dir[0].dot(dir[1])) > 0.9998f) //almost parallel, ca. 1 degree difference --> generate two contact points at ends
{
PxU32 numCons = 0;
PxReal segLenEps[2];
segLenEps[0] = segLen[0] * 0.001f;//0.1% error is ok.
segLenEps[1] = segLen[1] * 0.001f;
//project the two end points of each onto the axis of the other and take those 4 points.
//we could also generate a single normal at the single closest point, but this would be 'unstable'.
for (PxU32 destShapeIndex = 0; destShapeIndex < 2; destShapeIndex ++)
{
for (PxU32 startEnd = 0; startEnd < 2; startEnd ++)
{
const PxU32 srcShapeIndex = 1-destShapeIndex;
//project start/end of srcShapeIndex onto destShapeIndex.
PxVec3 pos[2];
pos[destShapeIndex] = startEnd ? segment[srcShapeIndex].p1 : segment[srcShapeIndex].p0;
const PxReal p = dir[destShapeIndex].dot(pos[destShapeIndex] - segment[destShapeIndex].p0);
if (p >= -segLenEps[destShapeIndex] && p <= (segLen[destShapeIndex] + segLenEps[destShapeIndex]))
{
pos[srcShapeIndex] = p * dir[destShapeIndex] + segment[destShapeIndex].p0;
PxVec3 normal = pos[1] - pos[0];
const PxReal normalLenSq = normal.magnitudeSquared();
if (normalLenSq > 1e-6 && normalLenSq < inflatedSumSquared)
{
const PxReal distance = PxSqrt(normalLenSq);
normal *= 1.0f/distance;
PxVec3 point = pos[1] - normal * (srcShapeIndex ? shapeCapsule1 : shapeCapsule0).radius;
point += transform0.p;
contactBuffer.contact(point, normal, distance - radiusSum);
numCons++;
}
}
}
}
if (numCons) //if we did not have contacts, then we may have the case where they are parallel, but are stacked end to end, in which case the old code will generate good contacts.
return true;
}
// Collision response
PxVec3 pos1 = segment[0].getPointAt(s);
PxVec3 pos2 = segment[1].getPointAt(t);
PxVec3 normal = pos1 - pos2;
const PxReal normalLenSq = normal.magnitudeSquared();
if (normalLenSq < 1e-6)
{
// Zero normal -> pick the direction of segment 0.
// Not always accurate but consistent with FW.
if (segLen[0] > 1e-6)
normal = dir[0];
else
normal = PxVec3(1.0f, 0.0f, 0.0f);
}
else
{
normal *= PxRecipSqrt(normalLenSq);
}
pos1 += transform0.p;
contactBuffer.contact(pos1 - normal * shapeCapsule0.radius, normal, PxSqrt(squareDist) - radiusSum);
return true;
}
return false;
}
