xuanchi/XCEngine
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
514dee10ccfd60c8f4b8d835b726ad293f2a8436
XCEngine/engine/third_party/physx/source/lowleveldynamics/src/DyConstraintSetup.cpp

810 lines
34 KiB
C++
Raw Blame History

// 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.
#include "foundation/PxMemory.h"
#include "foundation/PxMathUtils.h"
#include "DyConstraintPrep.h"
#include "DyCpuGpuArticulation.h"
#include "PxsRigidBody.h"
#include "DySolverConstraint1D.h"
#include "foundation/PxSort.h"
#include "DySolverConstraintDesc.h"
#include "PxcConstraintBlockStream.h"
#include "DyArticulationContactPrep.h"
#include "foundation/PxSIMDHelpers.h"
#include "DyArticulationUtils.h"
#include "DyAllocator.h"
namespace physx
{
namespace Dy
{
// dsequeira:
//
// we can choose any linear combination of equality constraints and get the same solution
// Hence we can orthogonalize the constraints using the inner product given by the
// inverse mass matrix, so that when we use PGS, solving a constraint row for a joint
// don't disturb the solution of prior rows.
//
// We also eliminate the equality constraints from the hard inequality constraints -
// (essentially projecting the direction corresponding to the lagrange multiplier
// onto the equality constraint subspace) but 'til I've verified this generates
// exactly the same KKT/complementarity conditions, status is 'experimental'.
//
// since for equality constraints the resulting rows have the property that applying
// an impulse along one row doesn't alter the projected velocity along another row,
// all equality constraints (plus one inequality constraint) can be processed in parallel
// using SIMD
//
// Eliminating the inequality constraints from each other would require a solver change
// and not give us any more parallelism, although we might get better convergence.
namespace
{
struct MassProps
{
FloatV invMass0; // the inverse mass of body0 after inverse mass scale was applied
FloatV invMass1; // the inverse mass of body1 after inverse mass scale was applied
FloatV invInertiaScale0;
FloatV invInertiaScale1;
PX_FORCE_INLINE MassProps(const PxReal imass0, const PxReal imass1, const PxConstraintInvMassScale& ims) :
invMass0(FLoad(imass0 * ims.linear0)),
invMass1(FLoad(imass1 * ims.linear1)),
invInertiaScale0(FLoad(ims.angular0)),
invInertiaScale1(FLoad(ims.angular1))
{}
};
PX_FORCE_INLINE PxReal innerProduct(const Px1DConstraint& row0, const Px1DConstraint& row1,
const PxVec4& row0AngSqrtInvInertia0, const PxVec4& row0AngSqrtInvInertia1,
const PxVec4& row1AngSqrtInvInertia0, const PxVec4& row1AngSqrtInvInertia1, const MassProps& m)
{
const Vec3V l0 = V3Mul(V3Scale(V3LoadA(row0.linear0), m.invMass0), V3LoadA(row1.linear0));
const Vec3V l1 = V3Mul(V3Scale(V3LoadA(row0.linear1), m.invMass1), V3LoadA(row1.linear1));
const Vec4V r0ang0 = V4LoadA(&row0AngSqrtInvInertia0.x);
const Vec4V r1ang0 = V4LoadA(&row1AngSqrtInvInertia0.x);
const Vec4V r0ang1 = V4LoadA(&row0AngSqrtInvInertia1.x);
const Vec4V r1ang1 = V4LoadA(&row1AngSqrtInvInertia1.x);
const Vec3V i0 = V3ScaleAdd(V3Mul(Vec3V_From_Vec4V(r0ang0), Vec3V_From_Vec4V(r1ang0)), m.invInertiaScale0, l0);
const Vec3V i1 = V3ScaleAdd(V3MulAdd(Vec3V_From_Vec4V(r0ang1), Vec3V_From_Vec4V(r1ang1), i0), m.invInertiaScale1, l1);
PxF32 f;
FStore(V3SumElems(i1), &f);
return f;
}
// indexed rotation around axis, with sine and cosine of half-angle
PX_FORCE_INLINE PxQuat indexedRotation(PxU32 axis, PxReal s, PxReal c)
{
PxQuat q(0,0,0,c);
reinterpret_cast<PxReal*>(&q)[axis] = s;
return q;
}
// PT: TODO: refactor with duplicate in FdMathUtils.cpp
PxQuat diagonalize(const PxMat33& m) // jacobi rotation using quaternions
{
const PxU32 MAX_ITERS = 5;
PxQuat q(PxIdentity);
PxMat33 d;
for(PxU32 i=0; i < MAX_ITERS;i++)
{
const PxMat33Padded axes(q);
d = axes.getTranspose() * m * axes;
const PxReal d0 = PxAbs(d[1][2]), d1 = PxAbs(d[0][2]), d2 = PxAbs(d[0][1]);
const PxU32 a = PxU32(d0 > d1 && d0 > d2 ? 0 : d1 > d2 ? 1 : 2); // rotation axis index, from largest off-diagonal element
const PxU32 a1 = PxGetNextIndex3(a), a2 = PxGetNextIndex3(a1);
if(d[a1][a2] == 0.0f || PxAbs(d[a1][a1] - d[a2][a2]) > 2e6f * PxAbs(2.0f * d[a1][a2]))
break;
const PxReal w = (d[a1][a1] - d[a2][a2]) / (2.0f * d[a1][a2]); // cot(2 * phi), where phi is the rotation angle
const PxReal absw = PxAbs(w);
PxQuat r;
if(absw > 1000)
r = indexedRotation(a, 1.0f / (4.0f * w), 1.0f); // h will be very close to 1, so use small angle approx instead
else
{
const PxReal t = 1.0f / (absw + PxSqrt(w * w + 1.0f)); // absolute value of tan phi
const PxReal h = 1.0f / PxSqrt(t * t + 1.0f); // absolute value of cos phi
PX_ASSERT(h != 1); // |w|<1000 guarantees this with typical IEEE754 machine eps (approx 6e-8)
r = indexedRotation(a, PxSqrt((1.0f - h) / 2.0f) * PxSign(w), PxSqrt((1.0f + h) / 2.0f));
}
q = (q * r).getNormalized();
}
return q;
}
PX_FORCE_INLINE void rescale(const Mat33V& m, PxVec3& a0, PxVec3& a1, PxVec3& a2)
{
const Vec3V va0 = V3LoadU(a0);
const Vec3V va1 = V3LoadU(a1);
const Vec3V va2 = V3LoadU(a2);
const Vec3V b0 = V3ScaleAdd(va0, V3GetX(m.col0), V3ScaleAdd(va1, V3GetY(m.col0), V3Scale(va2, V3GetZ(m.col0))));
const Vec3V b1 = V3ScaleAdd(va0, V3GetX(m.col1), V3ScaleAdd(va1, V3GetY(m.col1), V3Scale(va2, V3GetZ(m.col1))));
const Vec3V b2 = V3ScaleAdd(va0, V3GetX(m.col2), V3ScaleAdd(va1, V3GetY(m.col2), V3Scale(va2, V3GetZ(m.col2))));
V3StoreU(b0, a0);
V3StoreU(b1, a1);
V3StoreU(b2, a2);
}
PX_FORCE_INLINE void rescale4(const Mat33V& m, PxReal* a0, PxReal* a1, PxReal* a2)
{
const Vec4V va0 = V4LoadA(a0);
const Vec4V va1 = V4LoadA(a1);
const Vec4V va2 = V4LoadA(a2);
const Vec4V b0 = V4ScaleAdd(va0, V3GetX(m.col0), V4ScaleAdd(va1, V3GetY(m.col0), V4Scale(va2, V3GetZ(m.col0))));
const Vec4V b1 = V4ScaleAdd(va0, V3GetX(m.col1), V4ScaleAdd(va1, V3GetY(m.col1), V4Scale(va2, V3GetZ(m.col1))));
const Vec4V b2 = V4ScaleAdd(va0, V3GetX(m.col2), V4ScaleAdd(va1, V3GetY(m.col2), V4Scale(va2, V3GetZ(m.col2))));
V4StoreA(b0, a0);
V4StoreA(b1, a1);
V4StoreA(b2, a2);
}
void diagonalize(Px1DConstraint** row,
PxVec4* angSqrtInvInertia0,
PxVec4* angSqrtInvInertia1,
const MassProps& m)
{
const PxReal a00 = innerProduct(*row[0], *row[0], angSqrtInvInertia0[0], angSqrtInvInertia1[0], angSqrtInvInertia0[0], angSqrtInvInertia1[0], m);
const PxReal a01 = innerProduct(*row[0], *row[1], angSqrtInvInertia0[0], angSqrtInvInertia1[0], angSqrtInvInertia0[1], angSqrtInvInertia1[1], m);
const PxReal a02 = innerProduct(*row[0], *row[2], angSqrtInvInertia0[0], angSqrtInvInertia1[0], angSqrtInvInertia0[2], angSqrtInvInertia1[2], m);
const PxReal a11 = innerProduct(*row[1], *row[1], angSqrtInvInertia0[1], angSqrtInvInertia1[1], angSqrtInvInertia0[1], angSqrtInvInertia1[1], m);
const PxReal a12 = innerProduct(*row[1], *row[2], angSqrtInvInertia0[1], angSqrtInvInertia1[1], angSqrtInvInertia0[2], angSqrtInvInertia1[2], m);
const PxReal a22 = innerProduct(*row[2], *row[2], angSqrtInvInertia0[2], angSqrtInvInertia1[2], angSqrtInvInertia0[2], angSqrtInvInertia1[2], m);
const PxMat33 a(PxVec3(a00, a01, a02),
PxVec3(a01, a11, a12),
PxVec3(a02, a12, a22));
const PxQuat q = diagonalize(a);
const PxMat33 n(-q);
const Mat33V mn(V3LoadU(n.column0), V3LoadU(n.column1), V3LoadU(n.column2));
//KS - We treat as a Vec4V so that we get geometricError rescaled for free along with linear0
rescale4(mn, &row[0]->linear0.x, &row[1]->linear0.x, &row[2]->linear0.x);
rescale(mn, row[0]->linear1, row[1]->linear1, row[2]->linear1);
//KS - We treat as a PxVec4 so that we get velocityTarget rescaled for free
rescale4(mn, &row[0]->angular0.x, &row[1]->angular0.x, &row[2]->angular0.x);
rescale(mn, row[0]->angular1, row[1]->angular1, row[2]->angular1);
rescale4(mn, &angSqrtInvInertia0[0].x, &angSqrtInvInertia0[1].x, &angSqrtInvInertia0[2].x);
rescale4(mn, &angSqrtInvInertia1[0].x, &angSqrtInvInertia1[1].x, &angSqrtInvInertia1[2].x);
}
//
// A 1D constraint between two bodies (b0, b1) acts on specific linear and angular velocity
// directions of these two bodies. Let the constrained linear velocity direction for body b0
// be l0 and the constrained angular velocity direction be a0. Likewise, let l1 and a1 be
// the corresponding constrained velocity directions for body b1.
//
// Let the constraint Jacobian J be the 1x12 vector that combines the 3x1 vectors l0, a0, l1, a1
// J = | l0^T, a0^T, l1^T, a1^T |
//
// Let vl0, va0, vl1, va1 be the 3x1 linear/angular velocites of two bodies
// and v be the 12x1 combination of those:
//
// | vl0 |
// v = | va0 |
// | vl1 |
// | va1 |
//
// The constraint projected velocity scalar is then:
// projV = J * v
//
// Let M be the 12x12 mass matrix (with scalar masses m0, m1 and 3x3 inertias I0, I1)
//
// | m0 |
// | m0 |
// | m0 |
// | | | |
// | | I0 | |
// | | | |
// | m1 |
// | m1 |
// | m1 |
// | | | |
// | | I1 | |
// | | | |
//
// Let p be the impulse scalar that results from solving the 1D constraint given
// projV, geometric error etc.
// Turning this impulse p to a 12x1 delta velocity vector dv:
//
// dv = M^-1 * (J^T * p) = M^-1 * J^T * p
//
// Now to consider the case of multiple 1D constraints J0, J1, J2, ... operating
// on the same body pair.
//
// Let K be the matrix holding these constraints as rows
//
// | J0 |
// K = | J1 |
// | J2 |
// | ... |
//
// Applying these constraints:
//
// | | | p0 |
// | dv0 dv1 dv2 ... | = M^-1 * K^T * | p1 |
// | | | p2 |
//
// Let MK = (M^-1 * K^T)^T = K * M^-1 (M^-1 is symmetric). The transpose
// is only used here to talk about constraint rows instead of columns (since
// that expression seems to be used more commonly here).
//
// dvMatrix = MK^T * pMatrix
//
// The rows of MK define how an impulse affects the constrained velocity directions
// of the two bodies. Ideally, the different constraint rows operate on independent
// parts of the velocity such that constraints don't step on each others toe
// (constraint A making the situation better with respect to its own constrained
// velocity directions but worse for directions of constraint B and vice versa).
// A formal way to specify this goal is to try to have the rows of MK be orthogonal
// to each other, that is, to orthogonalize the MK matrix. This will eliminate
// any action of a constraint in directions that have been touched by previous
// constraints already. This re-configuration of constraint rows does not work in
// general but for hard equality constraints (no spring, targetVelocity=0,
// min/maxImpulse unlimited), changing the constraint rows to make them orthogonal
// should not change the solution of the constraint problem. As an example, one
// might consider a joint with two 1D constraints that lock linear movement in the
// xy-plane (those are hard equality constraints). It's fine to choose different
// constraint directions from the ones provided, assuming the new directions are
// still in the xy-plane and that the geometric errors get patched up accordingly.
//
// \param[in,out] row Pointers to the constraints to orthogonalize. The members
// linear0/1, angular0/1, geometricError and velocityTarget will
// get changed potentially
// \param[in,out] angSqrtInvInertia0 body b0 angular velocity directions of the
// constraints provided in parameter row but
// multiplied by the square root ot the inverse
// inertia tensor of body b0.
// I0^(-1/2) * angularDirection0, ...
// Will be replaced by the orthogonalized vectors.
// Note: the fourth component of the vectors serves
// no purpose in this method.
// \param[in,out] angSqrtInvInertia1 Same as previous parameter but for body b1.
// \param[in] rowCount Number of entries in row, angSqrtInvInertia0, angSqrtInvInertia1
// \param[in] eqRowCount Number of entries in row that represent equality constraints.
// The method expects the entries in row to be sorted by equality
// constraints first, followed by inequality constraints. The
// latter get orthogonalized relative to the equality constraints
// but not relative to the other inequality constraints.
// \param[in] m Some mass properties of the two bodies b0, b1.
//
void orthogonalize(Px1DConstraint** row,
PxVec4* angSqrtInvInertia0,
PxVec4* angSqrtInvInertia1,
PxU32 rowCount,
PxU32 eqRowCount,
const MassProps &m)
{
PX_ASSERT(eqRowCount<=6);
const FloatV zero = FZero();
Vec3V lin1m[6], ang1m[6], lin1[6], ang1[6];
Vec4V lin0m[6], ang0m[6]; // must have 0 in the W-field
Vec4V lin0AndG[6], ang0AndT[6];
for(PxU32 i=0;i<rowCount;i++)
{
Vec4V l0AndG = V4LoadA(&row[i]->linear0.x); // linear0 and geometric error
Vec4V a0AndT = V4LoadA(&row[i]->angular0.x); // angular0 and velocity target
Vec3V l1 = Vec3V_From_Vec4V(V4LoadA(&row[i]->linear1.x));
Vec3V a1 = Vec3V_From_Vec4V(V4LoadA(&row[i]->angular1.x));
Vec4V angSqrtL0 = V4LoadA(&angSqrtInvInertia0[i].x);
Vec4V angSqrtL1 = V4LoadA(&angSqrtInvInertia1[i].x);
const PxU32 eliminationRows = PxMin<PxU32>(i, eqRowCount);
for(PxU32 j=0;j<eliminationRows;j++)
{
//
// Gram-Schmidt algorithm to get orthogonal vectors. A set of vectors
// v0, v1, v2..., can be turned into orthogonal vectors u0, u1, u2, ...
// as follows:
//
// u0 = v0
// u1 = v1 - proj_u0(v1)
// u2 = v2 - proj_u0(v2) - proj_u1(v2)
// ...
//
// proj_u(v) denotes the resulting vector when vector v gets
// projected onto the normalized vector u.
//
// __ v
// /|
// /
// /
// /
// ----->---------------->
// proj_u(v) u
//
// Let <v,u> be the dot/inner product of the two vectors v and u.
//
// proj_u(v) = <v,normalize(u)> * normalize(u)
// = <v,u/|u|> * (u/|u|) = <v,u> / (|u|*|u|) * u
// = <v,u> / <u,u> * u
//
// The implementation here maps as follows:
//
// u = [orthoLinear0, orthoAngular0, orthoLinear1, orthoAngular1]
// v = [row[]->linear0, row[]->angular0, row[]->linear1, row[]->angular1]
//
// Since the solver is using momocity, orthogonality should not be achieved for rows
// M^-1 * u but for rows uM = M^(-1/2) * u (with M^(-1/2) being the square root of the
// inverse mass matrix). Following the described orthogonalization procedure to turn
// v1m into u1m that is orthogonal to u0m:
//
// u1M = v1M - proj_u0M(v1M)
// M^(-1/2) * u1 = M^(-1/2) * v1 - <v1M,u0M>/<u0M,u0M> * M^(-1/2) * u0
//
// Since M^(-1/2) is multiplied on the left and right hand side, this can be transformed to:
//
// u1 = v1 - <v1M,u0M>/<u0M,u0M> * u0
//
// For the computation of <v1M,u0M>/<u0M,u0M>, the following shall be considered:
//
// <vM,uM>
// = <M^(-1/2) * v, M^(-1/2) * u>
// = (M^(-1/2) * v)^T * (M^(-1/2) * u) (v and u being seen as 12x1 vectors here)
// = v^T * M^(-1/2)^T * M^(-1/2) * u
// = v^T * M^-1 * u (M^(-1/2) is a symmetric matrix, thus transposing has no effect)
// = <v, M^-1 * u>
//
// Applying this:
//
// <v1M,u0M>/<u0M,u0M> = <v1, M^-1 * u0> / <u0, M^-1 * u0>
//
// The code uses:
//
// v1m_ = [v1Lin0, I0^(-1/2) * v1Ang0, v1Lin1, I1^(-1/2) * v1Ang1]
// u0m_ = [(1/m0) * u0Lin0, I0^(-1/2) * u0Ang0, (1/m1) * u0Lin1, I1^(-1/2) * u0Ang1]
// u0m* = u0m_ / <u0M,u0M> (see variables named lin0m, ang0m, lin1m, ang1m in the code)
//
// And then does:
//
// <v1m_, u0m*> = <v1m_, u0m_> / <u0M,u0M> = <v, M^-1 * u> / <u0M,u0M> = <v1M,u0M>/<u0M,u0M>
// (see variable named t in the code)
//
// note: u0, u1, ... get computed for equality constraints. Inequality constraints do not generate new
// "base" vectors. Let's say u0, u1 are from equality constraints, then for inequality constraints
// u2, u3:
//
// u2 = v2 - proj_u0(v2) - proj_u1(v2)
// u3 = v3 - proj_u0(v3) - proj_u1(v3)
//
// in other words: the inequality constraints will be orthogonal to the equality constraints but not
// to other inequality constraints.
//
const Vec3V s0 = V3MulAdd(l1, lin1m[j], Vec3V_From_Vec4V_WUndefined(V4Mul(l0AndG, lin0m[j])));
const Vec3V s1 = V3MulAdd(Vec3V_From_Vec4V_WUndefined(angSqrtL1), ang1m[j], Vec3V_From_Vec4V_WUndefined(V4Mul(angSqrtL0, ang0m[j])));
const FloatV t = V3SumElems(V3Add(s0, s1));
l0AndG = V4NegScaleSub(lin0AndG[j], t, l0AndG); // note: this can reduce the error term by the amount covered by the orthogonal base vectors
a0AndT = V4NegScaleSub(ang0AndT[j], t, a0AndT); // note: for equality and inequality constraints, target velocity is expected to be 0
l1 = V3NegScaleSub(lin1[j], t, l1);
a1 = V3NegScaleSub(ang1[j], t, a1);
angSqrtL0 = V4NegScaleSub(V4LoadA(&angSqrtInvInertia0[j].x), t, angSqrtL0);
angSqrtL1 = V4NegScaleSub(V4LoadA(&angSqrtInvInertia1[j].x), t, angSqrtL1);
// note: angSqrtL1 is equivalent to I1^(-1/2) * a1 (and same goes for angSqrtL0)
}
V4StoreA(l0AndG, &row[i]->linear0.x);
V4StoreA(a0AndT, &row[i]->angular0.x);
V3StoreA(l1, row[i]->linear1);
V3StoreA(a1, row[i]->angular1);
V4StoreA(angSqrtL0, &angSqrtInvInertia0[i].x);
V4StoreA(angSqrtL1, &angSqrtInvInertia1[i].x);
if(i<eqRowCount)
{
lin0AndG[i] = l0AndG;
ang0AndT[i] = a0AndT;
lin1[i] = l1;
ang1[i] = a1;
//
// compute the base vector used for orthogonalization (see comments further above).
//
const Vec3V l0 = Vec3V_From_Vec4V(l0AndG);
const Vec3V l0m = V3Scale(l0, m.invMass0); // note that the invMass values used here have invMassScale applied already
const Vec3V l1m = V3Scale(l1, m.invMass1);
const Vec4V a0m = V4Scale(angSqrtL0, m.invInertiaScale0);
const Vec4V a1m = V4Scale(angSqrtL1, m.invInertiaScale1);
const Vec3V s0 = V3MulAdd(l0, l0m, V3Mul(l1, l1m));
const Vec4V s1 = V4MulAdd(a0m, angSqrtL0, V4Mul(a1m, angSqrtL1));
const FloatV s = V3SumElems(V3Add(s0, Vec3V_From_Vec4V_WUndefined(s1)));
const FloatV a = FSel(FIsGrtr(s, zero), FRecip(s), zero); // with mass scaling, it's possible for the inner product of a row to be zero
lin0m[i] = V4Scale(V4ClearW(Vec4V_From_Vec3V(l0m)), a);
ang0m[i] = V4Scale(V4ClearW(a0m), a);
lin1m[i] = V3Scale(l1m, a);
ang1m[i] = V3Scale(Vec3V_From_Vec4V_WUndefined(a1m), a);
}
}
}
}
// PT: make sure that there's at least a PxU32 after angular0/angular1 in the Px1DConstraint structure (for safe SIMD reads)
// Note that the code was V4LoadAding these before anyway so it must be safe already.
// PT: removed for now because some compilers didn't like it
//PX_COMPILE_TIME_ASSERT((sizeof(Px1DConstraint) - PX_OFFSET_OF_RT(Px1DConstraint, angular0)) >= (sizeof(PxVec3) + sizeof(PxU32)));
//PX_COMPILE_TIME_ASSERT((sizeof(Px1DConstraint) - PX_OFFSET_OF_RT(Px1DConstraint, angular1)) >= (sizeof(PxVec3) + sizeof(PxU32)));
// PT: TODO: move somewhere else
PX_FORCE_INLINE Vec3V M33MulV4(const Mat33V& a, const Vec4V b)
{
const FloatV x = V4GetX(b);
const FloatV y = V4GetY(b);
const FloatV z = V4GetZ(b);
const Vec3V v0 = V3Scale(a.col0, x);
const Vec3V v1 = V3Scale(a.col1, y);
const Vec3V v2 = V3Scale(a.col2, z);
const Vec3V v0PlusV1 = V3Add(v0, v1);
return V3Add(v0PlusV1, v2);
}
void preprocessRows(Px1DConstraint** sorted,
Px1DConstraint* rows,
PxVec4* angSqrtInvInertia0,
PxVec4* angSqrtInvInertia1,
PxU32 rowCount,
const PxMat33& sqrtInvInertia0F32,
const PxMat33& sqrtInvInertia1F32,
const PxReal invMass0,
const PxReal invMass1,
const PxConstraintInvMassScale& ims,
bool disablePreprocessing,
bool diagonalizeDrive)
{
// j is maxed at 12, typically around 7, so insertion sort is fine
for(PxU32 i=0; i<rowCount; i++)
{
Px1DConstraint* r = rows+i;
PxU32 j = i;
for(;j>0 && r->solveHint < sorted[j-1]->solveHint; j--)
sorted[j] = sorted[j-1];
sorted[j] = r;
}
for(PxU32 i=0;i<rowCount-1;i++)
PX_ASSERT(sorted[i]->solveHint <= sorted[i+1]->solveHint);
// PT: it is always safe to use "V3LoadU_SafeReadW" on the two first columns of a PxMat33. However in this case the passed matrices
// come from PxSolverBodyData::sqrtInvInertia (PGS) or PxTGSSolverBodyTxInertia::sqrtInvInertia (TGS). It is currently unsafe to use
// V3LoadU_SafeReadW in the TGS case (the matrix is the last element of the structure). So we keep V3LoadU here for now. For PGS we
// have a compile-time-assert (see PxSolverBodyData struct) to ensure safe reads on sqrtInvInertia.
// Note that because we only use this in M33MulV4 below, the ClearW calls could also be skipped.
const Mat33V sqrtInvInertia0 = Mat33V(
V3LoadU_SafeReadW(sqrtInvInertia0F32.column0),
V3LoadU_SafeReadW(sqrtInvInertia0F32.column1),
V3LoadU(sqrtInvInertia0F32.column2));
const Mat33V sqrtInvInertia1 = Mat33V(
V3LoadU_SafeReadW(sqrtInvInertia1F32.column0),
V3LoadU_SafeReadW(sqrtInvInertia1F32.column1),
V3LoadU(sqrtInvInertia1F32.column2));
PX_ASSERT(((uintptr_t(angSqrtInvInertia0)) & 0xF) == 0);
PX_ASSERT(((uintptr_t(angSqrtInvInertia1)) & 0xF) == 0);
for(PxU32 i=0; i<rowCount; ++i)
{
// PT: new version is 10 instructions smaller
//const Vec3V angDelta0_ = M33MulV3(sqrtInvInertia0, V3LoadU(sorted[i]->angular0));
//const Vec3V angDelta1_ = M33MulV3(sqrtInvInertia1, V3LoadU(sorted[i]->angular1));
const Vec3V angDelta0 = M33MulV4(sqrtInvInertia0, V4LoadA(&sorted[i]->angular0.x));
const Vec3V angDelta1 = M33MulV4(sqrtInvInertia1, V4LoadA(&sorted[i]->angular1.x));
V4StoreA(Vec4V_From_Vec3V(angDelta0), &angSqrtInvInertia0[i].x);
V4StoreA(Vec4V_From_Vec3V(angDelta1), &angSqrtInvInertia1[i].x);
}
if(disablePreprocessing)
return;
MassProps m(invMass0, invMass1, ims);
for(PxU32 i=0;i<rowCount;)
{
const PxU32 groupMajorId = PxU32(sorted[i]->solveHint>>8), start = i++;
while(i<rowCount && PxU32(sorted[i]->solveHint>>8) == groupMajorId)
i++;
if(groupMajorId == 4 || (groupMajorId == 8))
{
//
// PGS:
// - make all equality constraints orthogonal to each other
// - make all inequality constraints orthogonal to all equality constraints
// This assumes only PxConstraintSolveHint::eEQUALITY and ::eINEQUALITY is used.
//
// TGS:
// - make all linear equality constraints orthogonal to each other
// - make all linear inequality constraints orthogonal to all linear equality constraints
// - make all angular equality constraints orthogonal to each other
// - make all angular inequality constraints orthogonal to all angular equality constraints
// This is achieved by internally turning PxConstraintSolveHint::eEQUALITY and ::eINEQUALITY into
// ::eROTATIONAL_EQUALITY and ::eROTATIONAL_INEQUALITY for angular constraints.
//
PxU32 bCount = start; // count of bilateral constraints
for(; bCount<i && (sorted[bCount]->solveHint&255)==0; bCount++)
;
orthogonalize(sorted+start, angSqrtInvInertia0+start, angSqrtInvInertia1+start, i-start, bCount-start, m);
}
if(groupMajorId == 1 && diagonalizeDrive)
{
PxU32 slerp = start; // count of bilateral constraints
for(; slerp<i && (sorted[slerp]->solveHint&255)!=2; slerp++)
;
if(slerp+3 == i)
diagonalize(sorted+slerp, angSqrtInvInertia0+slerp, angSqrtInvInertia1+slerp, m);
PX_ASSERT(i-start==3);
diagonalize(sorted+start, angSqrtInvInertia0+start, angSqrtInvInertia1+start, m);
}
}
}
PxU32 ConstraintHelper::setupSolverConstraint(
PxSolverConstraintPrepDesc& prepDesc,
PxConstraintAllocator& allocator,
PxReal simDt, PxReal recipSimDt)
{
if (prepDesc.numRows == 0)
{
prepDesc.desc->constraint = NULL;
prepDesc.desc->writeBack = NULL;
prepDesc.desc->constraintLengthOver16 = 0;
return 0;
}
PxSolverConstraintDesc& desc = *prepDesc.desc;
const bool isExtended = (desc.linkIndexA != PxSolverConstraintDesc::RIGID_BODY)
|| (desc.linkIndexB != PxSolverConstraintDesc::RIGID_BODY);
const PxU32 stride = isExtended ? sizeof(SolverConstraint1DExt) : sizeof(SolverConstraint1D);
const PxU32 constraintLength = sizeof(SolverConstraint1DHeader) + stride * prepDesc.numRows;
//KS - +16 is for the constraint progress counter, which needs to be the last element in the constraint (so that we
//know SPU DMAs have completed)
PxU8* ptr = allocator.reserveConstraintData(constraintLength + 16u);
if(!checkConstraintDataPtr<true>(ptr))
return 0;
desc.constraint = ptr;
setConstraintLength(desc,constraintLength);
desc.writeBack = prepDesc.writeback;
PxMemSet(desc.constraint, 0, constraintLength);
SolverConstraint1DHeader* header = reinterpret_cast<SolverConstraint1DHeader*>(desc.constraint);
PxU8* constraints = desc.constraint + sizeof(SolverConstraint1DHeader);
init(*header, PxTo8(prepDesc.numRows), isExtended, prepDesc.invMassScales);
header->body0WorldOffset = prepDesc.body0WorldOffset;
header->linBreakImpulse = prepDesc.linBreakForce * simDt;
header->angBreakImpulse = prepDesc.angBreakForce * simDt;
header->breakable = PxU8((prepDesc.linBreakForce != PX_MAX_F32) || (prepDesc.angBreakForce != PX_MAX_F32));
header->invMass0D0 = prepDesc.data0->invMass * prepDesc.invMassScales.linear0;
header->invMass1D1 = prepDesc.data1->invMass * prepDesc.invMassScales.linear1;
PX_ALIGN(16, PxVec4) angSqrtInvInertia0[MAX_CONSTRAINT_ROWS];
PX_ALIGN(16, PxVec4) angSqrtInvInertia1[MAX_CONSTRAINT_ROWS];
Px1DConstraint* sorted[MAX_CONSTRAINT_ROWS];
preprocessRows(sorted, prepDesc.rows, angSqrtInvInertia0, angSqrtInvInertia1, prepDesc.numRows,
prepDesc.data0->sqrtInvInertia, prepDesc.data1->sqrtInvInertia, prepDesc.data0->invMass, prepDesc.data1->invMass,
prepDesc.invMassScales, isExtended || prepDesc.disablePreprocessing, prepDesc.improvedSlerp);
const PxReal erp = 1.0f;
PxU32 outCount = 0;
const SolverExtBody eb0(reinterpret_cast<const void*>(prepDesc.body0), prepDesc.data0, desc.linkIndexA);
const SolverExtBody eb1(reinterpret_cast<const void*>(prepDesc.body1), prepDesc.data1, desc.linkIndexB);
PxReal cfm = 0.f;
if (isExtended)
{
cfm = PxMax(eb0.getCFM(), eb1.getCFM());
}
for (PxU32 i = 0; i<prepDesc.numRows; i++)
{
PxPrefetchLine(constraints, 128);
SolverConstraint1D& s = *reinterpret_cast<SolverConstraint1D *>(constraints);
Px1DConstraint& c = *sorted[i];
PxReal minImpulse, maxImpulse;
computeMinMaxImpulseOrForceAsImpulse(
c.minImpulse, c.maxImpulse,
c.flags & Px1DConstraintFlag::eHAS_DRIVE_LIMIT, prepDesc.driveLimitsAreForces, simDt,
minImpulse, maxImpulse);
PxReal unitResponse;
PxReal jointSpeedForRestitutionBounce = 0.0f;
PxReal initJointSpeed = 0.0f;
const PxReal minResponseThreshold = prepDesc.minResponseThreshold;
if(!isExtended)
{
//The theoretical formulation of the Jacobian J has 4 terms {linear0, angular0, linear1, angular1}
//s.lin0 and s.lin1 match J.linear0 and J.linear1.
//We compute s.ang0 and s.ang1 but these *must not* be confused with the angular terms of the theoretical Jacobian.
//s.ang0 and s.ang1 are part of the momocity system that computes deltas to the angular motion that are neither
//angular momentum nor angular velocity.
//s.ang0 = I0^(-1/2) * J.angular0 with I0 denoting the inertia of body0 in the world frame.
//s.ang1 = I1^(-1/2) * J.angular1 with I1 denoting the inertia of body1 in the world frame.
//We then compute the unit response r = J * M^-1 * JTranspose with M denoting the mass matrix.
//r = (1/m0)*|J.linear0|^2 + (1/m1)*|J.linear1|^2 + J.angular0 * I0^-1 * J.angular0 + J.angular1 * I1^-1 * J.angular1
//We can write out the term [J.angular0 * I0^-1 * J.angular0] in a different way:
//J.angular0 * I0^-1 * J.angular0 = [J.angular0 * I0^(-1/2)] dot [I0^(-1/2) * J.angular0]
//Noting that s.ang0 = J.angular0 * I0^(-1/2) and the equivalent expression for body 1, we have the following:
//r = (1/m0)*|s.lin0|^2 + (1/m1)*|s.lin1|^2 + |s.ang0|^2 + |s.ang1|^2
//Init vel is computed using the standard Jacobian method because at this stage we have linear and angular velocities.
//The code that resolves the constraints instead accumulates delta linear velocities and delta angular momocities compatible with
//s.lin0/lin1 and s.ang0/ang1. Right now, though, we have only linear and angular velocities compatible with the theoretical
//Jacobian form:
//initVel = J.linear0.dot(linVel0) + J.angular0.dot(angvel0) - J.linear1.dot(linVel1) - J.angular1.dot(angvel1)
init(s, c.linear0, c.linear1, PxVec3(angSqrtInvInertia0[i].x, angSqrtInvInertia0[i].y, angSqrtInvInertia0[i].z),
PxVec3(angSqrtInvInertia1[i].x, angSqrtInvInertia1[i].y, angSqrtInvInertia1[i].z), minImpulse, maxImpulse);
s.ang0Writeback = c.angular0;
const PxReal resp0 = s.lin0.magnitudeSquared() * prepDesc.data0->invMass * prepDesc.invMassScales.linear0 + s.ang0.magnitudeSquared() * prepDesc.invMassScales.angular0;
const PxReal resp1 = s.lin1.magnitudeSquared() * prepDesc.data1->invMass * prepDesc.invMassScales.linear1 + s.ang1.magnitudeSquared() * prepDesc.invMassScales.angular1;
unitResponse = resp0 + resp1;
initJointSpeed = jointSpeedForRestitutionBounce = prepDesc.data0->projectVelocity(c.linear0, c.angular0) - prepDesc.data1->projectVelocity(c.linear1, c.angular1);
}
else
{
//this is articulation/deformable volume
init(s, c.linear0, c.linear1, c.angular0, c.angular1, minImpulse, maxImpulse);
SolverConstraint1DExt& e = static_cast<SolverConstraint1DExt&>(s);
const Cm::SpatialVector resp0 = createImpulseResponseVector(e.lin0, e.ang0, eb0);
const Cm::SpatialVector resp1 = createImpulseResponseVector(-e.lin1, -e.ang1, eb1);
unitResponse = getImpulseResponse(eb0, resp0, unsimdRef(e.deltaVA), prepDesc.invMassScales.linear0, prepDesc.invMassScales.angular0,
eb1, resp1, unsimdRef(e.deltaVB), prepDesc.invMassScales.linear1, prepDesc.invMassScales.angular1, false);
//Add CFM term!
if(unitResponse <= DY_ARTICULATION_MIN_RESPONSE)
continue;
unitResponse += cfm;
s.ang0Writeback = c.angular0;
s.lin0 = resp0.linear;
s.ang0 = resp0.angular;
s.lin1 = -resp1.linear;
s.ang1 = -resp1.angular;
PxReal vel0, vel1;
const bool b0IsRigidDynamic = (eb0.mLinkIndex == PxSolverConstraintDesc::RIGID_BODY);
const bool b1IsRigidDynamic = (eb1.mLinkIndex == PxSolverConstraintDesc::RIGID_BODY);
if(needsNormalVel(c) || b0IsRigidDynamic || b1IsRigidDynamic)
{
vel0 = eb0.projectVelocity(c.linear0, c.angular0);
vel1 = eb1.projectVelocity(c.linear1, c.angular1);
Dy::computeJointSpeedPGS(vel0, b0IsRigidDynamic, vel1, b1IsRigidDynamic, jointSpeedForRestitutionBounce, initJointSpeed);
}
//minResponseThreshold = PxMax(minResponseThreshold, DY_ARTICULATION_MIN_RESPONSE);
}
const PxReal recipUnitResponse = computeRecipUnitResponse(unitResponse, minResponseThreshold);
s.setSolverConstants(
compute1dConstraintSolverConstantsPGS(
c.flags,
c.mods.spring.stiffness, c.mods.spring.damping,
c.mods.bounce.restitution, c.mods.bounce.velocityThreshold,
c.geometricError, c.velocityTarget,
jointSpeedForRestitutionBounce, initJointSpeed,
unitResponse, recipUnitResponse,
erp,
simDt, recipSimDt));
if(c.flags & Px1DConstraintFlag::eOUTPUT_FORCE)
s.setOutputForceFlag(true);
outCount++;
constraints += stride;
}
//Reassign count to the header because we may have skipped some rows if they were degenerate
header->count = PxU8(outCount);
return prepDesc.numRows;
}
PxU32 SetupSolverConstraint(SolverConstraintShaderPrepDesc& shaderDesc,
PxSolverConstraintPrepDesc& prepDesc,
PxConstraintAllocator& allocator,
PxReal dt, PxReal invdt)
{
// LL shouldn't see broken constraints
PX_ASSERT(!(reinterpret_cast<ConstraintWriteback*>(prepDesc.writeback)->isBroken()));
setConstraintLength(*prepDesc.desc, 0);
if (!shaderDesc.solverPrep)
return 0;
//PxU32 numAxisConstraints = 0;
Px1DConstraint rows[MAX_CONSTRAINT_ROWS];
setupConstraintRows(rows, MAX_CONSTRAINT_ROWS);
prepDesc.invMassScales.linear0 = prepDesc.invMassScales.linear1 = prepDesc.invMassScales.angular0 = prepDesc.invMassScales.angular1 = 1.0f;
prepDesc.body0WorldOffset = PxVec3(0.0f);
PxVec3p unused_ra, unused_rb;
//TAG::solverprepcall
prepDesc.numRows = prepDesc.disableConstraint ? 0 : (*shaderDesc.solverPrep)(rows,
prepDesc.body0WorldOffset,
MAX_CONSTRAINT_ROWS,
prepDesc.invMassScales,
shaderDesc.constantBlock,
prepDesc.bodyFrame0, prepDesc.bodyFrame1, prepDesc.extendedLimits, unused_ra, unused_rb);
prepDesc.rows = rows;
return ConstraintHelper::setupSolverConstraint(prepDesc, allocator, dt, invdt);
}
}
}
Reference in New Issue View Git Blame Copy Permalink