986 lines
36 KiB
C++
986 lines
36 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-2021 NVIDIA Corporation. All rights reserved.
|
|
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
|
|
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
|
|
|
|
|
|
#include "foundation/PxPreprocessor.h"
|
|
#include "PsVecMath.h"
|
|
#include "PsFPU.h"
|
|
#include "CmPhysXCommon.h"
|
|
#include "DySolverBody.h"
|
|
#include "DySolverContactPF4.h"
|
|
#include "DySolverConstraint1D.h"
|
|
#include "DySolverConstraintDesc.h"
|
|
#include "DyThresholdTable.h"
|
|
#include "DySolverContext.h"
|
|
#include "PsUtilities.h"
|
|
#include "DyConstraint.h"
|
|
#include "PsAtomic.h"
|
|
#include "DySolverContact.h"
|
|
|
|
namespace physx
|
|
{
|
|
|
|
namespace Dy
|
|
{
|
|
|
|
static void solveContactCoulomb4_Block(const PxSolverConstraintDesc* PX_RESTRICT desc, SolverContext& /*cache*/)
|
|
{
|
|
PxSolverBody& b00 = *desc[0].bodyA;
|
|
PxSolverBody& b01 = *desc[0].bodyB;
|
|
PxSolverBody& b10 = *desc[1].bodyA;
|
|
PxSolverBody& b11 = *desc[1].bodyB;
|
|
PxSolverBody& b20 = *desc[2].bodyA;
|
|
PxSolverBody& b21 = *desc[2].bodyB;
|
|
PxSolverBody& b30 = *desc[3].bodyA;
|
|
PxSolverBody& b31 = *desc[3].bodyB;
|
|
|
|
//We'll need this.
|
|
const Vec4V vZero = V4Zero();
|
|
|
|
Vec4V linVel00 = V4LoadA(&b00.linearVelocity.x);
|
|
Vec4V linVel01 = V4LoadA(&b01.linearVelocity.x);
|
|
Vec4V angState00 = V4LoadA(&b00.angularState.x);
|
|
Vec4V angState01 = V4LoadA(&b01.angularState.x);
|
|
|
|
Vec4V linVel10 = V4LoadA(&b10.linearVelocity.x);
|
|
Vec4V linVel11 = V4LoadA(&b11.linearVelocity.x);
|
|
Vec4V angState10 = V4LoadA(&b10.angularState.x);
|
|
Vec4V angState11 = V4LoadA(&b11.angularState.x);
|
|
|
|
Vec4V linVel20 = V4LoadA(&b20.linearVelocity.x);
|
|
Vec4V linVel21 = V4LoadA(&b21.linearVelocity.x);
|
|
Vec4V angState20 = V4LoadA(&b20.angularState.x);
|
|
Vec4V angState21 = V4LoadA(&b21.angularState.x);
|
|
|
|
Vec4V linVel30 = V4LoadA(&b30.linearVelocity.x);
|
|
Vec4V linVel31 = V4LoadA(&b31.linearVelocity.x);
|
|
Vec4V angState30 = V4LoadA(&b30.angularState.x);
|
|
Vec4V angState31 = V4LoadA(&b31.angularState.x);
|
|
|
|
|
|
Vec4V linVel0T0, linVel0T1, linVel0T2, linVel0T3;
|
|
Vec4V linVel1T0, linVel1T1, linVel1T2, linVel1T3;
|
|
Vec4V angState0T0, angState0T1, angState0T2, angState0T3;
|
|
Vec4V angState1T0, angState1T1, angState1T2, angState1T3;
|
|
|
|
|
|
PX_TRANSPOSE_44(linVel00, linVel10, linVel20, linVel30, linVel0T0, linVel0T1, linVel0T2, linVel0T3);
|
|
PX_TRANSPOSE_44(linVel01, linVel11, linVel21, linVel31, linVel1T0, linVel1T1, linVel1T2, linVel1T3);
|
|
PX_TRANSPOSE_44(angState00, angState10, angState20, angState30, angState0T0, angState0T1, angState0T2, angState0T3);
|
|
PX_TRANSPOSE_44(angState01, angState11, angState21, angState31, angState1T0, angState1T1, angState1T2, angState1T3);
|
|
|
|
|
|
|
|
|
|
//hopefully pointer aliasing doesn't bite.
|
|
PxU8* PX_RESTRICT currPtr = desc[0].constraint;
|
|
|
|
SolverContactCoulombHeader4* PX_RESTRICT firstHeader = reinterpret_cast<SolverContactCoulombHeader4*>(currPtr);
|
|
|
|
const PxU8* PX_RESTRICT last = desc[0].constraint + firstHeader->frictionOffset;
|
|
|
|
//const PxU8* PX_RESTRICT endPtr = desc[0].constraint + getConstraintLength(desc[0]);
|
|
|
|
|
|
//TODO - can I avoid this many tests???
|
|
while(currPtr < last)
|
|
{
|
|
|
|
SolverContactCoulombHeader4* PX_RESTRICT hdr = reinterpret_cast<SolverContactCoulombHeader4*>(currPtr);
|
|
|
|
Vec4V* appliedForceBuffer = reinterpret_cast<Vec4V*>(currPtr + hdr->frictionOffset + sizeof(SolverFrictionHeader4));
|
|
|
|
//PX_ASSERT((PxU8*)appliedForceBuffer < endPtr);
|
|
|
|
currPtr = reinterpret_cast<PxU8*>(hdr + 1);
|
|
|
|
const PxU32 numNormalConstr = hdr->numNormalConstr;
|
|
|
|
SolverContact4Dynamic* PX_RESTRICT contacts = reinterpret_cast<SolverContact4Dynamic*>(currPtr);
|
|
//const Vec4V dominance1 = V4Neg(__dominance1);
|
|
|
|
currPtr = reinterpret_cast<PxU8*>(contacts + numNormalConstr);
|
|
|
|
const Vec4V invMass0D0 = hdr->invMassADom;
|
|
const Vec4V invMass1D1 = hdr->invMassBDom;
|
|
const Vec4V angD0 = hdr->angD0;
|
|
const Vec4V angD1 = hdr->angD1;
|
|
|
|
const Vec4V normalT0 = hdr->normalX;
|
|
const Vec4V normalT1 = hdr->normalY;
|
|
const Vec4V normalT2 = hdr->normalZ;
|
|
|
|
const Vec4V __normalVel1 = V4Mul(linVel0T0, normalT0);
|
|
const Vec4V __normalVel3 = V4Mul(linVel1T0, normalT0);
|
|
const Vec4V _normalVel1 = V4MulAdd(linVel0T1, normalT1, __normalVel1);
|
|
const Vec4V _normalVel3 = V4MulAdd(linVel1T1, normalT1, __normalVel3);
|
|
|
|
Vec4V normalVel1 = V4MulAdd(linVel0T2, normalT2, _normalVel1);
|
|
Vec4V normalVel3 = V4MulAdd(linVel1T2, normalT2, _normalVel3);
|
|
|
|
Vec4V accumDeltaF = vZero;
|
|
|
|
for(PxU32 i=0;i<numNormalConstr;i++)
|
|
{
|
|
SolverContact4Dynamic& c = contacts[i];
|
|
Ps::prefetchLine((&contacts[i+1]));
|
|
Ps::prefetchLine((&contacts[i+1]), 128);
|
|
Ps::prefetchLine((&contacts[i+1]), 256);
|
|
Ps::prefetchLine((&contacts[i+1]), 384);
|
|
|
|
const Vec4V appliedForce = c.appliedForce;
|
|
const Vec4V velMultiplier = c.velMultiplier;
|
|
|
|
const Vec4V targetVel = c.targetVelocity;
|
|
const Vec4V scaledBias = c.scaledBias;
|
|
const Vec4V maxImpulse = c.maxImpulse;
|
|
|
|
const Vec4V raXnT0 = c.raXnX;
|
|
const Vec4V raXnT1 = c.raXnY;
|
|
const Vec4V raXnT2 = c.raXnZ;
|
|
const Vec4V rbXnT0 = c.rbXnX;
|
|
const Vec4V rbXnT1 = c.rbXnY;
|
|
const Vec4V rbXnT2 = c.rbXnZ;
|
|
|
|
|
|
const Vec4V __normalVel2 = V4Mul(raXnT0, angState0T0);
|
|
const Vec4V __normalVel4 = V4Mul(rbXnT0, angState1T0);
|
|
|
|
|
|
const Vec4V _normalVel2 = V4MulAdd(raXnT1, angState0T1, __normalVel2);
|
|
const Vec4V _normalVel4 = V4MulAdd(rbXnT1, angState1T1, __normalVel4);
|
|
|
|
|
|
const Vec4V normalVel2 = V4MulAdd(raXnT2, angState0T2, _normalVel2);
|
|
const Vec4V normalVel4 = V4MulAdd(rbXnT2, angState1T2, _normalVel4);
|
|
|
|
const Vec4V biasedErr = V4MulAdd(targetVel, velMultiplier, V4Neg(scaledBias));
|
|
|
|
//Linear component - normal * invMass_dom
|
|
|
|
const Vec4V _normalVel(V4Add(normalVel1, normalVel2));
|
|
const Vec4V __normalVel(V4Add(normalVel3, normalVel4));
|
|
|
|
const Vec4V normalVel = V4Sub(_normalVel, __normalVel );
|
|
|
|
const Vec4V _deltaF = V4NegMulSub(normalVel, velMultiplier, biasedErr);
|
|
const Vec4V nAppliedForce = V4Neg(appliedForce);
|
|
const Vec4V _deltaF2 = V4Max(_deltaF, nAppliedForce);
|
|
const Vec4V _newAppliedForce(V4Add(appliedForce, _deltaF2));
|
|
const Vec4V newAppliedForce = V4Min(_newAppliedForce, maxImpulse);
|
|
const Vec4V deltaF = V4Sub(newAppliedForce, appliedForce);
|
|
|
|
normalVel1 = V4MulAdd(invMass0D0, deltaF, normalVel1);
|
|
normalVel3 = V4NegMulSub(invMass1D1, deltaF, normalVel3);
|
|
|
|
accumDeltaF = V4Add(deltaF, accumDeltaF);
|
|
|
|
const Vec4V deltaFAng0 = V4Mul(angD0, deltaF);
|
|
const Vec4V deltaFAng1 = V4Mul(angD1, deltaF);
|
|
|
|
angState0T0 = V4MulAdd(raXnT0, deltaFAng0, angState0T0);
|
|
angState1T0 = V4NegMulSub(rbXnT0, deltaFAng1, angState1T0);
|
|
|
|
angState0T1 = V4MulAdd(raXnT1, deltaFAng0, angState0T1);
|
|
angState1T1 = V4NegMulSub(rbXnT1, deltaFAng1, angState1T1);
|
|
|
|
angState0T2 = V4MulAdd(raXnT2, deltaFAng0, angState0T2);
|
|
angState1T2 = V4NegMulSub(rbXnT2, deltaFAng1, angState1T2);
|
|
|
|
c.appliedForce = newAppliedForce;
|
|
appliedForceBuffer[i] = newAppliedForce;
|
|
}
|
|
|
|
const Vec4V accumDeltaF0 = V4Mul(accumDeltaF, invMass0D0);
|
|
const Vec4V accumDeltaF1 = V4Mul(accumDeltaF, invMass1D1);
|
|
|
|
linVel0T0 = V4MulAdd(normalT0, accumDeltaF0, linVel0T0);
|
|
linVel1T0 = V4NegMulSub(normalT0, accumDeltaF1, linVel1T0);
|
|
linVel0T1 = V4MulAdd(normalT1, accumDeltaF0, linVel0T1);
|
|
linVel1T1 = V4NegMulSub(normalT1, accumDeltaF1, linVel1T1);
|
|
linVel0T2 = V4MulAdd(normalT2, accumDeltaF0, linVel0T2);
|
|
linVel1T2 = V4NegMulSub(normalT2, accumDeltaF1, linVel1T2);
|
|
}
|
|
|
|
PX_ASSERT(currPtr == last);
|
|
|
|
|
|
//KS - we need to use PX_TRANSPOSE_44 here instead of the 34_43 variants because the W components are being used to
|
|
//store the bodies' progress counters.
|
|
|
|
PX_TRANSPOSE_44(linVel0T0, linVel0T1, linVel0T2, linVel0T3, linVel00, linVel10, linVel20, linVel30);
|
|
PX_TRANSPOSE_44(linVel1T0, linVel1T1, linVel1T2, linVel1T3, linVel01, linVel11, linVel21, linVel31);
|
|
PX_TRANSPOSE_44(angState0T0, angState0T1, angState0T2, angState0T3, angState00, angState10, angState20, angState30);
|
|
PX_TRANSPOSE_44(angState1T0, angState1T1, angState1T2, angState1T3, angState01, angState11, angState21, angState31);
|
|
|
|
|
|
// Write back
|
|
V4StoreA(linVel00, &b00.linearVelocity.x);
|
|
V4StoreA(linVel10, &b10.linearVelocity.x);
|
|
V4StoreA(linVel20, &b20.linearVelocity.x);
|
|
V4StoreA(linVel30, &b30.linearVelocity.x);
|
|
|
|
V4StoreA(linVel01, &b01.linearVelocity.x);
|
|
V4StoreA(linVel11, &b11.linearVelocity.x);
|
|
V4StoreA(linVel21, &b21.linearVelocity.x);
|
|
V4StoreA(linVel31, &b31.linearVelocity.x);
|
|
|
|
V4StoreA(angState00, &b00.angularState.x);
|
|
V4StoreA(angState10, &b10.angularState.x);
|
|
V4StoreA(angState20, &b20.angularState.x);
|
|
V4StoreA(angState30, &b30.angularState.x);
|
|
|
|
V4StoreA(angState01, &b01.angularState.x);
|
|
V4StoreA(angState11, &b11.angularState.x);
|
|
V4StoreA(angState21, &b21.angularState.x);
|
|
V4StoreA(angState31, &b31.angularState.x);
|
|
}
|
|
|
|
|
|
static void solveContactCoulomb4_StaticBlock(const PxSolverConstraintDesc* PX_RESTRICT desc, SolverContext& /*cache*/)
|
|
{
|
|
PxSolverBody& b00 = *desc[0].bodyA;
|
|
PxSolverBody& b10 = *desc[1].bodyA;
|
|
PxSolverBody& b20 = *desc[2].bodyA;
|
|
PxSolverBody& b30 = *desc[3].bodyA;
|
|
|
|
//We'll need this.
|
|
const Vec4V vZero = V4Zero();
|
|
|
|
Vec4V linVel00 = V4LoadA(&b00.linearVelocity.x);
|
|
Vec4V angState00 = V4LoadA(&b00.angularState.x);
|
|
|
|
Vec4V linVel10 = V4LoadA(&b10.linearVelocity.x);
|
|
Vec4V angState10 = V4LoadA(&b10.angularState.x);
|
|
|
|
Vec4V linVel20 = V4LoadA(&b20.linearVelocity.x);
|
|
Vec4V angState20 = V4LoadA(&b20.angularState.x);
|
|
|
|
Vec4V linVel30 = V4LoadA(&b30.linearVelocity.x);
|
|
Vec4V angState30 = V4LoadA(&b30.angularState.x);
|
|
|
|
|
|
Vec4V linVel0T0, linVel0T1, linVel0T2, linVel0T3;
|
|
Vec4V angState0T0, angState0T1, angState0T2, angState0T3;
|
|
|
|
|
|
PX_TRANSPOSE_44(linVel00, linVel10, linVel20, linVel30, linVel0T0, linVel0T1, linVel0T2, linVel0T3);
|
|
PX_TRANSPOSE_44(angState00, angState10, angState20, angState30, angState0T0, angState0T1, angState0T2, angState0T3);
|
|
|
|
|
|
//hopefully pointer aliasing doesn't bite.
|
|
PxU8* PX_RESTRICT currPtr = desc[0].constraint;
|
|
|
|
SolverContactCoulombHeader4* PX_RESTRICT firstHeader = reinterpret_cast<SolverContactCoulombHeader4*>(currPtr);
|
|
|
|
const PxU8* PX_RESTRICT last = desc[0].constraint + firstHeader->frictionOffset;
|
|
|
|
|
|
//TODO - can I avoid this many tests???
|
|
while(currPtr < last)
|
|
{
|
|
|
|
SolverContactCoulombHeader4* PX_RESTRICT hdr = reinterpret_cast<SolverContactCoulombHeader4*>(currPtr);
|
|
|
|
Vec4V* appliedForceBuffer = reinterpret_cast<Vec4V*>(currPtr + hdr->frictionOffset + sizeof(SolverFrictionHeader4));
|
|
|
|
currPtr = reinterpret_cast<PxU8*>(hdr + 1);
|
|
|
|
const PxU32 numNormalConstr = hdr->numNormalConstr;
|
|
|
|
SolverContact4Base* PX_RESTRICT contacts = reinterpret_cast<SolverContact4Base*>(currPtr);
|
|
|
|
currPtr = reinterpret_cast<PxU8*>(contacts + numNormalConstr);
|
|
|
|
const Vec4V invMass0D0 = hdr->invMassADom;
|
|
const Vec4V angD0 = hdr->angD0;
|
|
|
|
const Vec4V normalT0 = hdr->normalX;
|
|
const Vec4V normalT1 = hdr->normalY;
|
|
const Vec4V normalT2 = hdr->normalZ;
|
|
|
|
const Vec4V __normalVel1 = V4Mul(linVel0T0, normalT0);
|
|
const Vec4V _normalVel1 = V4MulAdd(linVel0T1, normalT1, __normalVel1);
|
|
|
|
Vec4V normalVel1 = V4MulAdd(linVel0T2, normalT2, _normalVel1);
|
|
|
|
Vec4V accumDeltaF = vZero;
|
|
|
|
for(PxU32 i=0;i<numNormalConstr;i++)
|
|
{
|
|
SolverContact4Base& c = contacts[i];
|
|
Ps::prefetchLine((&contacts[i+1]));
|
|
Ps::prefetchLine((&contacts[i+1]), 128);
|
|
Ps::prefetchLine((&contacts[i+1]), 256);
|
|
|
|
const Vec4V appliedForce = c.appliedForce;
|
|
const Vec4V velMultiplier = c.velMultiplier;
|
|
|
|
const Vec4V targetVel = c.targetVelocity;
|
|
const Vec4V scaledBias = c.scaledBias;
|
|
const Vec4V maxImpulse = c.maxImpulse;
|
|
|
|
const Vec4V raXnT0 = c.raXnX;
|
|
const Vec4V raXnT1 = c.raXnY;
|
|
const Vec4V raXnT2 = c.raXnZ;
|
|
|
|
|
|
const Vec4V __normalVel2 = V4Mul(raXnT0, angState0T0);
|
|
|
|
const Vec4V _normalVel2 = V4MulAdd(raXnT1, angState0T1, __normalVel2);
|
|
|
|
const Vec4V normalVel2 = V4MulAdd(raXnT2, angState0T2, _normalVel2);
|
|
|
|
const Vec4V biasedErr = V4MulAdd(targetVel, velMultiplier, V4Neg(scaledBias));
|
|
|
|
//Linear component - normal * invMass_dom
|
|
|
|
const Vec4V normalVel(V4Add(normalVel1, normalVel2));
|
|
|
|
const Vec4V _deltaF = V4NegMulSub(normalVel, velMultiplier, biasedErr);
|
|
const Vec4V nAppliedForce = V4Neg(appliedForce);
|
|
|
|
const Vec4V _deltaF2 = V4Max(_deltaF, nAppliedForce);
|
|
|
|
const Vec4V _newAppliedForce(V4Add(appliedForce, _deltaF2));
|
|
const Vec4V newAppliedForce = V4Min(_newAppliedForce, maxImpulse);
|
|
const Vec4V deltaF = V4Sub(newAppliedForce, appliedForce);
|
|
const Vec4V deltaAngF = V4Mul(deltaF, angD0);
|
|
|
|
normalVel1 = V4MulAdd(invMass0D0, deltaF, normalVel1);
|
|
|
|
accumDeltaF = V4Add(deltaF, accumDeltaF);
|
|
|
|
angState0T0 = V4MulAdd(raXnT0, deltaAngF, angState0T0);
|
|
angState0T1 = V4MulAdd(raXnT1, deltaAngF, angState0T1);
|
|
angState0T2 = V4MulAdd(raXnT2, deltaAngF, angState0T2);
|
|
|
|
c.appliedForce = newAppliedForce;
|
|
appliedForceBuffer[i] = newAppliedForce;
|
|
}
|
|
const Vec4V scaledAccumDeltaF = V4Mul(accumDeltaF, invMass0D0);
|
|
linVel0T0 = V4MulAdd(normalT0, scaledAccumDeltaF, linVel0T0);
|
|
linVel0T1 = V4MulAdd(normalT1, scaledAccumDeltaF, linVel0T1);
|
|
linVel0T2 = V4MulAdd(normalT2, scaledAccumDeltaF, linVel0T2);
|
|
}
|
|
|
|
PX_ASSERT(currPtr == last);
|
|
|
|
//KS - we need to use PX_TRANSPOSE_44 here instead of the 34_43 variants because the W components are being used to
|
|
//store the bodies' progress counters.
|
|
|
|
PX_TRANSPOSE_44(linVel0T0, linVel0T1, linVel0T2, linVel0T3, linVel00, linVel10, linVel20, linVel30);
|
|
PX_TRANSPOSE_44(angState0T0, angState0T1, angState0T2, angState0T3, angState00, angState10, angState20, angState30);
|
|
|
|
// Write back
|
|
// Write back
|
|
V4StoreA(linVel00, &b00.linearVelocity.x);
|
|
V4StoreA(linVel10, &b10.linearVelocity.x);
|
|
V4StoreA(linVel20, &b20.linearVelocity.x);
|
|
V4StoreA(linVel30, &b30.linearVelocity.x);
|
|
|
|
V4StoreA(angState00, &b00.angularState.x);
|
|
V4StoreA(angState10, &b10.angularState.x);
|
|
V4StoreA(angState20, &b20.angularState.x);
|
|
V4StoreA(angState30, &b30.angularState.x);
|
|
}
|
|
|
|
static void solveFriction4_Block(const PxSolverConstraintDesc* PX_RESTRICT desc, SolverContext& /*cache*/)
|
|
{
|
|
PxSolverBody& b00 = *desc[0].bodyA;
|
|
PxSolverBody& b01 = *desc[0].bodyB;
|
|
PxSolverBody& b10 = *desc[1].bodyA;
|
|
PxSolverBody& b11 = *desc[1].bodyB;
|
|
PxSolverBody& b20 = *desc[2].bodyA;
|
|
PxSolverBody& b21 = *desc[2].bodyB;
|
|
PxSolverBody& b30 = *desc[3].bodyA;
|
|
PxSolverBody& b31 = *desc[3].bodyB;
|
|
|
|
|
|
Vec4V linVel00 = V4LoadA(&b00.linearVelocity.x);
|
|
Vec4V linVel01 = V4LoadA(&b01.linearVelocity.x);
|
|
Vec4V angState00 = V4LoadA(&b00.angularState.x);
|
|
Vec4V angState01 = V4LoadA(&b01.angularState.x);
|
|
|
|
Vec4V linVel10 = V4LoadA(&b10.linearVelocity.x);
|
|
Vec4V linVel11 = V4LoadA(&b11.linearVelocity.x);
|
|
Vec4V angState10 = V4LoadA(&b10.angularState.x);
|
|
Vec4V angState11 = V4LoadA(&b11.angularState.x);
|
|
|
|
Vec4V linVel20 = V4LoadA(&b20.linearVelocity.x);
|
|
Vec4V linVel21 = V4LoadA(&b21.linearVelocity.x);
|
|
Vec4V angState20 = V4LoadA(&b20.angularState.x);
|
|
Vec4V angState21 = V4LoadA(&b21.angularState.x);
|
|
|
|
Vec4V linVel30 = V4LoadA(&b30.linearVelocity.x);
|
|
Vec4V linVel31 = V4LoadA(&b31.linearVelocity.x);
|
|
Vec4V angState30 = V4LoadA(&b30.angularState.x);
|
|
Vec4V angState31 = V4LoadA(&b31.angularState.x);
|
|
|
|
|
|
Vec4V linVel0T0, linVel0T1, linVel0T2, linVel0T3;
|
|
Vec4V linVel1T0, linVel1T1, linVel1T2, linVel1T3;
|
|
Vec4V angState0T0, angState0T1, angState0T2, angState0T3;
|
|
Vec4V angState1T0, angState1T1, angState1T2, angState1T3;
|
|
|
|
|
|
PX_TRANSPOSE_44(linVel00, linVel10, linVel20, linVel30, linVel0T0, linVel0T1, linVel0T2, linVel0T3);
|
|
PX_TRANSPOSE_44(linVel01, linVel11, linVel21, linVel31, linVel1T0, linVel1T1, linVel1T2, linVel1T3);
|
|
PX_TRANSPOSE_44(angState00, angState10, angState20, angState30, angState0T0, angState0T1, angState0T2, angState0T3);
|
|
PX_TRANSPOSE_44(angState01, angState11, angState21, angState31, angState1T0, angState1T1, angState1T2, angState1T3);
|
|
|
|
PxU8* PX_RESTRICT currPtr = desc[0].constraint;
|
|
PxU8* PX_RESTRICT endPtr = desc[0].constraint + getConstraintLength(desc[0]);
|
|
|
|
|
|
while(currPtr < endPtr)
|
|
{
|
|
SolverFrictionHeader4* PX_RESTRICT hdr = reinterpret_cast<SolverFrictionHeader4*>(currPtr);
|
|
|
|
currPtr = reinterpret_cast<PxU8*>(hdr + 1);
|
|
|
|
Vec4V* appliedImpulses = reinterpret_cast<Vec4V*>(currPtr);
|
|
|
|
currPtr += hdr->numNormalConstr * sizeof(Vec4V);
|
|
|
|
Ps::prefetchLine(currPtr, 128);
|
|
Ps::prefetchLine(currPtr,256);
|
|
Ps::prefetchLine(currPtr,384);
|
|
|
|
const PxU32 numFrictionConstr = hdr->numFrictionConstr;
|
|
|
|
SolverFriction4Dynamic* PX_RESTRICT frictions = reinterpret_cast<SolverFriction4Dynamic*>(currPtr);
|
|
|
|
currPtr = reinterpret_cast<PxU8*>(frictions + hdr->numFrictionConstr);
|
|
|
|
const PxU32 maxFrictionConstr = numFrictionConstr;
|
|
|
|
const Vec4V staticFric = hdr->staticFriction;
|
|
|
|
const Vec4V invMass0D0 = hdr->invMassADom;
|
|
const Vec4V invMass1D1 = hdr->invMassBDom;
|
|
|
|
const Vec4V angD0 = hdr->angD0;
|
|
const Vec4V angD1 = hdr->angD1;
|
|
|
|
for(PxU32 i=0;i<maxFrictionConstr;i++)
|
|
{
|
|
SolverFriction4Dynamic& f = frictions[i];
|
|
Ps::prefetchLine((&f)+1);
|
|
Ps::prefetchLine((&f)+1,128);
|
|
Ps::prefetchLine((&f)+1,256);
|
|
Ps::prefetchLine((&f)+1,384);
|
|
|
|
const Vec4V appliedImpulse = appliedImpulses[i>>hdr->frictionPerContact];
|
|
|
|
const Vec4V maxFriction = V4Mul(staticFric, appliedImpulse);
|
|
|
|
const Vec4V nMaxFriction = V4Neg(maxFriction);
|
|
|
|
const Vec4V normalX = f.normalX;
|
|
const Vec4V normalY = f.normalY;
|
|
const Vec4V normalZ = f.normalZ;
|
|
|
|
const Vec4V raXnX = f.raXnX;
|
|
const Vec4V raXnY = f.raXnY;
|
|
const Vec4V raXnZ = f.raXnZ;
|
|
|
|
const Vec4V rbXnX = f.rbXnX;
|
|
const Vec4V rbXnY = f.rbXnY;
|
|
const Vec4V rbXnZ = f.rbXnZ;
|
|
|
|
const Vec4V appliedForce(f.appliedForce);
|
|
const Vec4V velMultiplier(f.velMultiplier);
|
|
const Vec4V targetVel(f.targetVelocity);
|
|
|
|
//4 x 4 Dot3 products encoded as 8 M44 transposes, 4 MulV and 8 MulAdd ops
|
|
|
|
const Vec4V __normalVel1 = V4Mul(linVel0T0, normalX);
|
|
const Vec4V __normalVel2 = V4Mul(raXnX, angState0T0);
|
|
const Vec4V __normalVel3 = V4Mul(linVel1T0, normalX);
|
|
const Vec4V __normalVel4 = V4Mul(rbXnX, angState1T0);
|
|
|
|
const Vec4V _normalVel1 = V4MulAdd(linVel0T1, normalY, __normalVel1);
|
|
const Vec4V _normalVel2 = V4MulAdd(raXnY, angState0T1, __normalVel2);
|
|
const Vec4V _normalVel3 = V4MulAdd(linVel1T1, normalY, __normalVel3);
|
|
const Vec4V _normalVel4 = V4MulAdd(rbXnY, angState1T1, __normalVel4);
|
|
|
|
const Vec4V normalVel1 = V4MulAdd(linVel0T2, normalZ, _normalVel1);
|
|
const Vec4V normalVel2 = V4MulAdd(raXnZ, angState0T2, _normalVel2);
|
|
const Vec4V normalVel3 = V4MulAdd(linVel1T2, normalZ, _normalVel3);
|
|
const Vec4V normalVel4 = V4MulAdd(rbXnZ, angState1T2, _normalVel4);
|
|
|
|
|
|
const Vec4V _normalVel = V4Add(normalVel1, normalVel2);
|
|
const Vec4V __normalVel = V4Add(normalVel3, normalVel4);
|
|
|
|
const Vec4V normalVel = V4Sub(_normalVel, __normalVel );
|
|
|
|
const Vec4V tmp = V4NegMulSub(targetVel, velMultiplier, appliedForce);
|
|
Vec4V newAppliedForce = V4MulAdd(normalVel, velMultiplier, tmp);
|
|
newAppliedForce = V4Clamp(newAppliedForce,nMaxFriction, maxFriction);
|
|
const Vec4V deltaF = V4Sub(newAppliedForce, appliedForce);
|
|
|
|
const Vec4V deltaLinF0 = V4Mul(invMass0D0, deltaF);
|
|
const Vec4V deltaLinF1 = V4Mul(invMass1D1, deltaF);
|
|
|
|
const Vec4V deltaAngF0 = V4Mul(angD0, deltaF);
|
|
const Vec4V deltaAngF1 = V4Mul(angD1, deltaF);
|
|
|
|
|
|
linVel0T0 = V4MulAdd(normalX, deltaLinF0, linVel0T0);
|
|
linVel1T0 = V4NegMulSub(normalX, deltaLinF1, linVel1T0);
|
|
angState0T0 = V4MulAdd(raXnX, deltaAngF0, angState0T0);
|
|
angState1T0 = V4NegMulSub(rbXnX, deltaAngF1, angState1T0);
|
|
|
|
linVel0T1 = V4MulAdd(normalY, deltaLinF0, linVel0T1);
|
|
linVel1T1 = V4NegMulSub(normalY, deltaLinF1, linVel1T1);
|
|
angState0T1 = V4MulAdd(raXnY, deltaAngF0, angState0T1);
|
|
angState1T1 = V4NegMulSub(rbXnY, deltaAngF1, angState1T1);
|
|
|
|
linVel0T2 = V4MulAdd(normalZ, deltaLinF0, linVel0T2);
|
|
linVel1T2 = V4NegMulSub(normalZ, deltaLinF1, linVel1T2);
|
|
angState0T2 = V4MulAdd(raXnZ, deltaAngF0, angState0T2);
|
|
angState1T2 = V4NegMulSub(rbXnZ, deltaAngF1, angState1T2);
|
|
|
|
f.appliedForce = newAppliedForce;
|
|
}
|
|
}
|
|
|
|
PX_ASSERT(currPtr == endPtr);
|
|
|
|
//KS - we need to use PX_TRANSPOSE_44 here instead of the 34_43 variants because the W components are being used to
|
|
//store the bodies' progress counters.
|
|
|
|
PX_TRANSPOSE_44(linVel0T0, linVel0T1, linVel0T2, linVel0T3, linVel00, linVel10, linVel20, linVel30);
|
|
PX_TRANSPOSE_44(linVel1T0, linVel1T1, linVel1T2, linVel1T3, linVel01, linVel11, linVel21, linVel31);
|
|
PX_TRANSPOSE_44(angState0T0, angState0T1, angState0T2, angState0T3, angState00, angState10, angState20, angState30);
|
|
PX_TRANSPOSE_44(angState1T0, angState1T1, angState1T2, angState1T3, angState01, angState11, angState21, angState31);
|
|
|
|
|
|
// Write back
|
|
// Write back
|
|
V4StoreA(linVel00, &b00.linearVelocity.x);
|
|
V4StoreA(linVel10, &b10.linearVelocity.x);
|
|
V4StoreA(linVel20, &b20.linearVelocity.x);
|
|
V4StoreA(linVel30, &b30.linearVelocity.x);
|
|
|
|
V4StoreA(linVel01, &b01.linearVelocity.x);
|
|
V4StoreA(linVel11, &b11.linearVelocity.x);
|
|
V4StoreA(linVel21, &b21.linearVelocity.x);
|
|
V4StoreA(linVel31, &b31.linearVelocity.x);
|
|
|
|
V4StoreA(angState00, &b00.angularState.x);
|
|
V4StoreA(angState10, &b10.angularState.x);
|
|
V4StoreA(angState20, &b20.angularState.x);
|
|
V4StoreA(angState30, &b30.angularState.x);
|
|
|
|
V4StoreA(angState01, &b01.angularState.x);
|
|
V4StoreA(angState11, &b11.angularState.x);
|
|
V4StoreA(angState21, &b21.angularState.x);
|
|
V4StoreA(angState31, &b31.angularState.x);
|
|
|
|
}
|
|
|
|
|
|
static void solveFriction4_StaticBlock(const PxSolverConstraintDesc* PX_RESTRICT desc, SolverContext& /*cache*/)
|
|
{
|
|
|
|
PxSolverBody& b00 = *desc[0].bodyA;
|
|
PxSolverBody& b10 = *desc[1].bodyA;
|
|
PxSolverBody& b20 = *desc[2].bodyA;
|
|
PxSolverBody& b30 = *desc[3].bodyA;
|
|
|
|
|
|
Vec4V linVel00 = V4LoadA(&b00.linearVelocity.x);
|
|
Vec4V angState00 = V4LoadA(&b00.angularState.x);
|
|
|
|
Vec4V linVel10 = V4LoadA(&b10.linearVelocity.x);
|
|
Vec4V angState10 = V4LoadA(&b10.angularState.x);
|
|
|
|
Vec4V linVel20 = V4LoadA(&b20.linearVelocity.x);
|
|
Vec4V angState20 = V4LoadA(&b20.angularState.x);
|
|
|
|
Vec4V linVel30 = V4LoadA(&b30.linearVelocity.x);
|
|
Vec4V angState30 = V4LoadA(&b30.angularState.x);
|
|
|
|
|
|
Vec4V linVel0T0, linVel0T1, linVel0T2, linVel0T3;
|
|
Vec4V angState0T0, angState0T1, angState0T2, angState0T3;
|
|
|
|
|
|
PX_TRANSPOSE_44(linVel00, linVel10, linVel20, linVel30, linVel0T0, linVel0T1, linVel0T2, linVel0T3);
|
|
PX_TRANSPOSE_44(angState00, angState10, angState20, angState30, angState0T0, angState0T1, angState0T2, angState0T3);
|
|
|
|
PxU8* PX_RESTRICT currPtr = desc[0].constraint;
|
|
PxU8* PX_RESTRICT endPtr = desc[0].constraint + getConstraintLength(desc[0]);
|
|
|
|
|
|
while(currPtr < endPtr)
|
|
{
|
|
SolverFrictionHeader4* PX_RESTRICT hdr = reinterpret_cast<SolverFrictionHeader4*>(currPtr);
|
|
|
|
currPtr = reinterpret_cast<PxU8*>(hdr + 1);
|
|
|
|
Vec4V* appliedImpulses = reinterpret_cast<Vec4V*>(currPtr);
|
|
|
|
currPtr += hdr->numNormalConstr * sizeof(Vec4V);
|
|
|
|
Ps::prefetchLine(currPtr, 128);
|
|
Ps::prefetchLine(currPtr,256);
|
|
Ps::prefetchLine(currPtr,384);
|
|
|
|
const PxU32 numFrictionConstr = hdr->numFrictionConstr;
|
|
|
|
SolverFriction4Base* PX_RESTRICT frictions = reinterpret_cast<SolverFriction4Base*>(currPtr);
|
|
|
|
currPtr = reinterpret_cast<PxU8*>(frictions + hdr->numFrictionConstr);
|
|
|
|
const PxU32 maxFrictionConstr = numFrictionConstr;
|
|
|
|
const Vec4V staticFric = hdr->staticFriction;
|
|
|
|
const Vec4V invMass0D0 = hdr->invMassADom;
|
|
const Vec4V angD0 = hdr->angD0;
|
|
|
|
for(PxU32 i=0;i<maxFrictionConstr;i++)
|
|
{
|
|
SolverFriction4Base& f = frictions[i];
|
|
Ps::prefetchLine((&f)+1);
|
|
Ps::prefetchLine((&f)+1,128);
|
|
Ps::prefetchLine((&f)+1,256);
|
|
|
|
const Vec4V appliedImpulse = appliedImpulses[i>>hdr->frictionPerContact];
|
|
|
|
const Vec4V maxFriction = V4Mul(staticFric, appliedImpulse);
|
|
|
|
const Vec4V nMaxFriction = V4Neg(maxFriction);
|
|
|
|
const Vec4V normalX = f.normalX;
|
|
const Vec4V normalY = f.normalY;
|
|
const Vec4V normalZ = f.normalZ;
|
|
|
|
const Vec4V raXnX = f.raXnX;
|
|
const Vec4V raXnY = f.raXnY;
|
|
const Vec4V raXnZ = f.raXnZ;
|
|
|
|
const Vec4V appliedForce(f.appliedForce);
|
|
const Vec4V velMultiplier(f.velMultiplier);
|
|
const Vec4V targetVel(f.targetVelocity);
|
|
|
|
//4 x 4 Dot3 products encoded as 8 M44 transposes, 4 MulV and 8 MulAdd ops
|
|
|
|
const Vec4V __normalVel1 = V4Mul(linVel0T0, normalX);
|
|
const Vec4V __normalVel2 = V4Mul(raXnX, angState0T0);
|
|
|
|
const Vec4V _normalVel1 = V4MulAdd(linVel0T1, normalY, __normalVel1);
|
|
const Vec4V _normalVel2 = V4MulAdd(raXnY, angState0T1, __normalVel2);
|
|
|
|
const Vec4V normalVel1 = V4MulAdd(linVel0T2, normalZ, _normalVel1);
|
|
const Vec4V normalVel2 = V4MulAdd(raXnZ, angState0T2, _normalVel2);
|
|
|
|
const Vec4V delLinVel00 = V4Mul(normalX, invMass0D0);
|
|
|
|
const Vec4V delLinVel10 = V4Mul(normalY, invMass0D0);
|
|
|
|
const Vec4V normalVel = V4Add(normalVel1, normalVel2);
|
|
|
|
const Vec4V delLinVel20 = V4Mul(normalZ, invMass0D0);
|
|
|
|
const Vec4V tmp = V4NegMulSub(targetVel, velMultiplier, appliedForce);
|
|
|
|
Vec4V newAppliedForce = V4MulAdd(normalVel, velMultiplier, tmp);
|
|
newAppliedForce = V4Clamp(newAppliedForce,nMaxFriction, maxFriction);
|
|
const Vec4V deltaF = V4Sub(newAppliedForce, appliedForce);
|
|
|
|
const Vec4V deltaAngF0 = V4Mul(angD0, deltaF);
|
|
|
|
linVel0T0 = V4MulAdd(delLinVel00, deltaF, linVel0T0);
|
|
angState0T0 = V4MulAdd(raXnX, deltaAngF0, angState0T0);
|
|
|
|
linVel0T1 = V4MulAdd(delLinVel10, deltaF, linVel0T1);
|
|
angState0T1 = V4MulAdd(raXnY, deltaAngF0, angState0T1);
|
|
|
|
linVel0T2 = V4MulAdd(delLinVel20, deltaF, linVel0T2);
|
|
angState0T2 = V4MulAdd(raXnZ, deltaAngF0, angState0T2);
|
|
|
|
f.appliedForce = newAppliedForce;
|
|
}
|
|
}
|
|
|
|
PX_ASSERT(currPtr == endPtr);
|
|
|
|
//KS - we need to use PX_TRANSPOSE_44 here instead of the 34_43 variants because the W components are being used to
|
|
//store the bodies' progress counters.
|
|
|
|
PX_TRANSPOSE_44(linVel0T0, linVel0T1, linVel0T2, linVel0T3, linVel00, linVel10, linVel20, linVel30);
|
|
PX_TRANSPOSE_44(angState0T0, angState0T1, angState0T2, angState0T3, angState00, angState10, angState20, angState30);
|
|
|
|
// Write back
|
|
// Write back
|
|
V4StoreA(linVel00, &b00.linearVelocity.x);
|
|
V4StoreA(linVel10, &b10.linearVelocity.x);
|
|
V4StoreA(linVel20, &b20.linearVelocity.x);
|
|
V4StoreA(linVel30, &b30.linearVelocity.x);
|
|
|
|
V4StoreA(angState00, &b00.angularState.x);
|
|
V4StoreA(angState10, &b10.angularState.x);
|
|
V4StoreA(angState20, &b20.angularState.x);
|
|
V4StoreA(angState30, &b30.angularState.x);
|
|
}
|
|
|
|
static void concludeContactCoulomb4(const PxSolverConstraintDesc* desc, SolverContext& /*cache*/)
|
|
{
|
|
PxU8* PX_RESTRICT cPtr = desc[0].constraint;
|
|
|
|
const Vec4V zero = V4Zero();
|
|
|
|
const SolverContactCoulombHeader4* PX_RESTRICT firstHeader = reinterpret_cast<const SolverContactCoulombHeader4*>(cPtr);
|
|
PxU8* PX_RESTRICT last = desc[0].constraint + firstHeader->frictionOffset;
|
|
|
|
PxU32 pointStride = firstHeader->type == DY_SC_TYPE_BLOCK_RB_CONTACT ? sizeof(SolverContact4Dynamic) : sizeof(SolverContact4Base);
|
|
|
|
while(cPtr < last)
|
|
{
|
|
const SolverContactCoulombHeader4* PX_RESTRICT hdr = reinterpret_cast<const SolverContactCoulombHeader4*>(cPtr);
|
|
cPtr += sizeof(SolverContactCoulombHeader4);
|
|
|
|
const PxU32 numNormalConstr = hdr->numNormalConstr;
|
|
|
|
//if(cPtr < last)
|
|
//Ps::prefetchLine(cPtr, 512);
|
|
Ps::prefetchLine(cPtr,128);
|
|
Ps::prefetchLine(cPtr,256);
|
|
Ps::prefetchLine(cPtr,384);
|
|
|
|
for(PxU32 i=0;i<numNormalConstr;i++)
|
|
{
|
|
SolverContact4Base *c = reinterpret_cast<SolverContact4Base*>(cPtr);
|
|
cPtr += pointStride;
|
|
c->scaledBias = V4Max(c->scaledBias, zero);
|
|
}
|
|
}
|
|
PX_ASSERT(cPtr == last);
|
|
}
|
|
|
|
void writeBackContactCoulomb4(const PxSolverConstraintDesc* desc, SolverContext& cache,
|
|
const PxSolverBodyData** PX_RESTRICT bd0, const PxSolverBodyData** PX_RESTRICT bd1)
|
|
{
|
|
Vec4V normalForceV = V4Zero();
|
|
PxU8* PX_RESTRICT cPtr = desc[0].constraint;
|
|
PxReal* PX_RESTRICT vForceWriteback0 = reinterpret_cast<PxReal*>(desc[0].writeBack);
|
|
PxReal* PX_RESTRICT vForceWriteback1 = reinterpret_cast<PxReal*>(desc[1].writeBack);
|
|
PxReal* PX_RESTRICT vForceWriteback2 = reinterpret_cast<PxReal*>(desc[2].writeBack);
|
|
PxReal* PX_RESTRICT vForceWriteback3 = reinterpret_cast<PxReal*>(desc[3].writeBack);
|
|
|
|
const SolverContactCoulombHeader4* PX_RESTRICT firstHeader = reinterpret_cast<const SolverContactCoulombHeader4*>(cPtr);
|
|
PxU8* PX_RESTRICT last = desc[0].constraint + firstHeader->frictionOffset;
|
|
|
|
const PxU32 pointStride = firstHeader->type == DY_SC_TYPE_BLOCK_RB_CONTACT ? sizeof(SolverContact4Dynamic)
|
|
: sizeof(SolverContact4Base);
|
|
|
|
bool writeBackThresholds[4] = {false, false, false, false};
|
|
|
|
|
|
while(cPtr < last)
|
|
{
|
|
const SolverContactCoulombHeader4* PX_RESTRICT hdr = reinterpret_cast<const SolverContactCoulombHeader4*>(cPtr);
|
|
cPtr += sizeof(SolverContactCoulombHeader4);
|
|
|
|
writeBackThresholds[0] = hdr->flags[0] & SolverContactHeader::eHAS_FORCE_THRESHOLDS;
|
|
writeBackThresholds[1] = hdr->flags[1] & SolverContactHeader::eHAS_FORCE_THRESHOLDS;
|
|
writeBackThresholds[2] = hdr->flags[2] & SolverContactHeader::eHAS_FORCE_THRESHOLDS;
|
|
writeBackThresholds[3] = hdr->flags[3] & SolverContactHeader::eHAS_FORCE_THRESHOLDS;
|
|
|
|
const PxU32 numNormalConstr = hdr->numNormalConstr;
|
|
|
|
Ps::prefetchLine(cPtr, 256);
|
|
Ps::prefetchLine(cPtr, 384);
|
|
|
|
|
|
for(PxU32 i=0; i<numNormalConstr; i++)
|
|
{
|
|
SolverContact4Base* c = reinterpret_cast<SolverContact4Base*>(cPtr);
|
|
cPtr += pointStride;
|
|
|
|
const Vec4V appliedForce = c->appliedForce;
|
|
if(vForceWriteback0 && i < hdr->numNormalConstr0)
|
|
FStore(V4GetX(appliedForce), vForceWriteback0++);
|
|
if(vForceWriteback1 && i < hdr->numNormalConstr1)
|
|
FStore(V4GetY(appliedForce), vForceWriteback1++);
|
|
if(vForceWriteback2 && i < hdr->numNormalConstr2)
|
|
FStore(V4GetZ(appliedForce), vForceWriteback2++);
|
|
if(vForceWriteback3 && i < hdr->numNormalConstr3)
|
|
FStore(V4GetW(appliedForce), vForceWriteback3++);
|
|
|
|
normalForceV = V4Add(normalForceV, appliedForce);
|
|
}
|
|
}
|
|
PX_ASSERT(cPtr == last);
|
|
|
|
PX_ALIGN(16, PxReal nf[4]);
|
|
V4StoreA(normalForceV, nf);
|
|
|
|
//all constraint pointer in descs are the same constraint
|
|
Sc::ShapeInteraction** shapeInteractions = reinterpret_cast<SolverContactCoulombHeader4*>(desc[0].constraint)->shapeInteraction;
|
|
|
|
for(PxU32 a = 0; a < 4; ++a)
|
|
{
|
|
if(writeBackThresholds[a] && desc[a].linkIndexA == PxSolverConstraintDesc::NO_LINK && desc[a].linkIndexB == PxSolverConstraintDesc::NO_LINK &&
|
|
nf[a] !=0.f && (bd0[a]->reportThreshold < PX_MAX_REAL || bd1[a]->reportThreshold < PX_MAX_REAL))
|
|
{
|
|
ThresholdStreamElement elt;
|
|
elt.normalForce = nf[a];
|
|
elt.threshold = PxMin<float>(bd0[a]->reportThreshold, bd1[a]->reportThreshold);
|
|
elt.nodeIndexA = IG::NodeIndex(bd0[a]->nodeIndex);
|
|
elt.nodeIndexB = IG::NodeIndex(bd1[a]->nodeIndex);
|
|
elt.shapeInteraction = shapeInteractions[a];
|
|
Ps::order(elt.nodeIndexA, elt.nodeIndexB);
|
|
PX_ASSERT(elt.nodeIndexA < elt.nodeIndexB);
|
|
PX_ASSERT(cache.mThresholdStreamIndex<cache.mThresholdStreamLength);
|
|
cache.mThresholdStream[cache.mThresholdStreamIndex++] = elt;
|
|
}
|
|
}
|
|
}
|
|
|
|
void solveContactCoulombPreBlock(const PxSolverConstraintDesc* PX_RESTRICT desc, const PxU32 /*constraintCount*/, SolverContext& cache)
|
|
{
|
|
solveContactCoulomb4_Block(desc, cache);
|
|
}
|
|
|
|
void solveContactCoulombPreBlock_Static(const PxSolverConstraintDesc* PX_RESTRICT desc, const PxU32 /*constraintCount*/, SolverContext& cache)
|
|
{
|
|
solveContactCoulomb4_StaticBlock(desc, cache);
|
|
}
|
|
|
|
void solveContactCoulombPreBlock_Conclude(const PxSolverConstraintDesc* PX_RESTRICT desc, const PxU32 /*constraintCount*/, SolverContext& cache)
|
|
{
|
|
solveContactCoulomb4_Block(desc, cache);
|
|
concludeContactCoulomb4(desc, cache);
|
|
}
|
|
|
|
void solveContactCoulombPreBlock_ConcludeStatic(const PxSolverConstraintDesc* PX_RESTRICT desc, const PxU32 /*constraintCount*/, SolverContext& cache)
|
|
{
|
|
solveContactCoulomb4_StaticBlock(desc, cache);
|
|
concludeContactCoulomb4(desc, cache);
|
|
}
|
|
|
|
void solveContactCoulombPreBlock_WriteBack(const PxSolverConstraintDesc* PX_RESTRICT desc, const PxU32 /*constraintCount*/, SolverContext& cache)
|
|
{
|
|
solveContactCoulomb4_Block(desc, cache);
|
|
|
|
const PxSolverBodyData* bd0[4] = { &cache.solverBodyArray[desc[0].bodyADataIndex],
|
|
&cache.solverBodyArray[desc[1].bodyADataIndex],
|
|
&cache.solverBodyArray[desc[2].bodyADataIndex],
|
|
&cache.solverBodyArray[desc[3].bodyADataIndex]};
|
|
|
|
const PxSolverBodyData* bd1[4] = { &cache.solverBodyArray[desc[0].bodyBDataIndex],
|
|
&cache.solverBodyArray[desc[1].bodyBDataIndex],
|
|
&cache.solverBodyArray[desc[2].bodyBDataIndex],
|
|
&cache.solverBodyArray[desc[3].bodyBDataIndex]};
|
|
|
|
|
|
|
|
writeBackContactCoulomb4(desc, cache, bd0, bd1);
|
|
|
|
if(cache.mThresholdStreamIndex > (cache.mThresholdStreamLength - 4))
|
|
{
|
|
//Write back to global buffer
|
|
PxI32 threshIndex = physx::shdfnd::atomicAdd(cache.mSharedOutThresholdPairs, PxI32(cache.mThresholdStreamIndex)) - PxI32(cache.mThresholdStreamIndex);
|
|
for(PxU32 a = 0; a < cache.mThresholdStreamIndex; ++a)
|
|
{
|
|
cache.mSharedThresholdStream[a + threshIndex] = cache.mThresholdStream[a];
|
|
}
|
|
cache.mThresholdStreamIndex = 0;
|
|
}
|
|
}
|
|
|
|
void solveContactCoulombPreBlock_WriteBackStatic(const PxSolverConstraintDesc* PX_RESTRICT desc, const PxU32 /*constraintCount*/, SolverContext& cache)
|
|
{
|
|
solveContactCoulomb4_StaticBlock(desc, cache);
|
|
const PxSolverBodyData* bd0[4] = { &cache.solverBodyArray[desc[0].bodyADataIndex],
|
|
&cache.solverBodyArray[desc[1].bodyADataIndex],
|
|
&cache.solverBodyArray[desc[2].bodyADataIndex],
|
|
&cache.solverBodyArray[desc[3].bodyADataIndex]};
|
|
|
|
const PxSolverBodyData* bd1[4] = { &cache.solverBodyArray[desc[0].bodyBDataIndex],
|
|
&cache.solverBodyArray[desc[1].bodyBDataIndex],
|
|
&cache.solverBodyArray[desc[2].bodyBDataIndex],
|
|
&cache.solverBodyArray[desc[3].bodyBDataIndex]};
|
|
|
|
writeBackContactCoulomb4(desc, cache, bd0, bd1);
|
|
|
|
if(cache.mThresholdStreamIndex > (cache.mThresholdStreamLength - 4))
|
|
{
|
|
//Write back to global buffer
|
|
PxI32 threshIndex = physx::shdfnd::atomicAdd(cache.mSharedOutThresholdPairs, PxI32(cache.mThresholdStreamIndex)) - PxI32(cache.mThresholdStreamIndex);
|
|
for(PxU32 a = 0; a < cache.mThresholdStreamIndex; ++a)
|
|
{
|
|
cache.mSharedThresholdStream[a + threshIndex] = cache.mThresholdStream[a];
|
|
}
|
|
cache.mThresholdStreamIndex = 0;
|
|
}
|
|
}
|
|
|
|
void solveFrictionCoulombPreBlock(const PxSolverConstraintDesc* PX_RESTRICT desc, const PxU32 /*constraintCount*/, SolverContext& cache)
|
|
{
|
|
solveFriction4_Block(desc, cache);
|
|
}
|
|
|
|
void solveFrictionCoulombPreBlock_Static(const PxSolverConstraintDesc* PX_RESTRICT desc, const PxU32 /*constraintCount*/, SolverContext& cache)
|
|
{
|
|
solveFriction4_StaticBlock(desc, cache);
|
|
}
|
|
|
|
void solveFrictionCoulombPreBlock_Conclude(const PxSolverConstraintDesc* PX_RESTRICT desc, const PxU32 /*constraintCount*/, SolverContext& cache)
|
|
{
|
|
solveFriction4_Block(desc, cache);
|
|
}
|
|
|
|
void solveFrictionCoulombPreBlock_ConcludeStatic(const PxSolverConstraintDesc* PX_RESTRICT desc, const PxU32 /*constraintCount*/, SolverContext& cache)
|
|
{
|
|
solveFriction4_StaticBlock(desc, cache);
|
|
}
|
|
|
|
void solveFrictionCoulombPreBlock_WriteBack(const PxSolverConstraintDesc* PX_RESTRICT desc, const PxU32 /*constraintCount*/, SolverContext& cache)
|
|
{
|
|
solveFriction4_Block(desc, cache);
|
|
}
|
|
|
|
void solveFrictionCoulombPreBlock_WriteBackStatic(const PxSolverConstraintDesc* PX_RESTRICT desc, const PxU32 /*constraintCount*/, SolverContext& cache)
|
|
{
|
|
solveFriction4_StaticBlock(desc, cache);
|
|
}
|
|
|
|
|
|
}
|
|
|
|
}
|
|
|