From 3dfe2108cfab31ba3ee5527e217d0d8e99a51162 Mon Sep 17 00:00:00 2001 From: git perforce import user Date: Tue, 25 Oct 2016 12:29:14 -0600 Subject: Initial commit: PhysX 3.4.0 Update @ 21294896 APEX 1.4.0 Update @ 21275617 [CL 21300167] --- .../LowLevelDynamics/src/DyArticulationScalar.cpp | 575 +++++++++++++++++++++ 1 file changed, 575 insertions(+) create mode 100644 PhysX_3.4/Source/LowLevelDynamics/src/DyArticulationScalar.cpp (limited to 'PhysX_3.4/Source/LowLevelDynamics/src/DyArticulationScalar.cpp') diff --git a/PhysX_3.4/Source/LowLevelDynamics/src/DyArticulationScalar.cpp b/PhysX_3.4/Source/LowLevelDynamics/src/DyArticulationScalar.cpp new file mode 100644 index 00000000..af00a367 --- /dev/null +++ b/PhysX_3.4/Source/LowLevelDynamics/src/DyArticulationScalar.cpp @@ -0,0 +1,575 @@ +// This code contains NVIDIA Confidential Information and is disclosed to you +// under a form of NVIDIA software license agreement provided separately to you. +// +// Notice +// NVIDIA Corporation and its licensors retain all intellectual property and +// proprietary rights in and to this software and related documentation and +// any modifications thereto. Any use, reproduction, disclosure, or +// distribution of this software and related documentation without an express +// license agreement from NVIDIA Corporation is strictly prohibited. +// +// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES +// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO +// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT, +// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE. +// +// Information and code furnished is believed to be accurate and reliable. +// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such +// information or for any infringement of patents or other rights of third parties that may +// result from its use. No license is granted by implication or otherwise under any patent +// or patent rights of NVIDIA Corporation. Details are subject to change without notice. +// This code supersedes and replaces all information previously supplied. +// NVIDIA Corporation products are not authorized for use as critical +// components in life support devices or systems without express written approval of +// NVIDIA Corporation. +// +// Copyright (c) 2008-2016 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 "DyArticulationUtils.h" +#include "DyArticulationScalar.h" +#include "DyArticulationReference.h" +#include "DyArticulationFnsDebug.h" + +namespace physx +{ +namespace Dy +{ +namespace ArticulationRef +{ + Cm::SpatialVector propagateImpulse(const FsRow& row, + const FsJointVectors& j, + PxVec3& SZ, + const Cm::SpatialVector& Z, + const FsRowAux& aux) + { + typedef ArticulationFnsScalar Fns; + + SZ = Fns::axisDot(reinterpret_cast(aux.S), Z); + return Fns::translateForce(getParentOffset(j), Z - Fns::axisMultiply(getDSI(row), SZ)); + } + + Cm::SpatialVector propagateVelocity(const FsRow& row, + const FsJointVectors& j, + const PxVec3& SZ, + const Cm::SpatialVector& v, + const FsRowAux& aux) + { + typedef ArticulationFnsScalar Fns; + + Cm::SpatialVector w = Fns::translateMotion(-getParentOffset(j), v); + PxVec3 DSZ = Fns::multiply(row.D, SZ); + + return w - Fns::axisMultiply(reinterpret_cast(aux.S), DSZ + Fns::axisDot(getDSI(row), w)); + } + + void applyImpulse(const FsData& matrix, + Cm::SpatialVector* velocity, + PxU32 linkID, + const Cm::SpatialVector& impulse) + { + typedef ArticulationFnsScalar Fns; + + PX_ASSERT(matrix.linkCount<=DY_ARTICULATION_MAX_SIZE); + + const FsRow* rows = getFsRows(matrix); + const FsRowAux* aux = getAux(matrix); + const FsJointVectors* jointVectors = getJointVectors(matrix); + + Cm::SpatialVector dV[DY_ARTICULATION_MAX_SIZE]; + PxVec3 SZ[DY_ARTICULATION_MAX_SIZE]; + + for(PxU32 i=0;i0;) + { + LtbRow& b = rows[i]; + inertia[i] = Fns::invertInertia(inertia[i]); + PxU32 p = m.parent[i]; + + Cm::SpatialVector* j0 = &reinterpret_cast(*b.j0), + * j1 = &reinterpret_cast(*b.j1); + + Fns::multiply(j, inertia[i], j1); + PxMat33 jResponse = Fns::invertSym33(-Fns::multiplySym(j, j1)); + j1[0] = j[0]; j1[1] = j[1]; j1[2] = j[2]; + + b.jResponse = Mat33V_From_PxMat33(jResponse); + Fns::multiply(j, j0, jResponse); + inertia[p] = Fns::multiplySubtract(inertia[p], j, j0); + j0[0] = j[0]; j0[1] = j[1]; j0[2] = j[2]; + } + + rows[0].inertia = Fns::invertInertia(inertia[0]); + for(PxU32 i=1;i(c); + const LtbRow* rows = getLtbRows(m); + PxMemZero(y, m.linkCount*sizeof(Cm::SpatialVector)); + + for(PxU32 i=m.linkCount;i-->1;) + { + PxU32 p = m.parent[i]; + const LtbRow& r = rows[i]; + b[i] -= PxVec4(Fns::axisDot(&static_cast(*r.j1), y[i]),0); + y[p] -= Fns::axisMultiply(&static_cast(*r.j0), b[i].getXYZ()); + } + + y[0] = Fns::multiply(rows[0].inertia,y[0]); + + for(PxU32 i=1; i(*r.j0), y[p]); + y[i] = Fns::multiply(r.inertia, y[i]) - Fns::axisMultiply(&static_cast(*r.j1), t); + } +} + +void PxcFsPropagateDrivenInertiaScalar(FsData& matrix, + const FsInertia* baseInertia, + const PxReal* isf, + const Mat33V* load) +{ + typedef ArticulationFnsScalar Fns; + + Cm::SpatialVector IS[3], DSI[3]; + PxMat33 D; + + FsRow* rows = getFsRows(matrix); + const FsRowAux* aux = getAux(matrix); + const FsJointVectors* jointVectors = getJointVectors(matrix); + + SpInertia inertia[DY_ARTICULATION_MAX_SIZE]; + for(PxU32 i=0;i0;) + { + FsRow& r = rows[i]; + const FsRowAux& a = aux[i]; + const FsJointVectors& jv = jointVectors[i]; + + Fns::multiply(IS, inertia[i], &static_cast(*a.S)); + + PX_ALIGN(16, PxMat33) L; + PxMat33_From_Mat33V(load[i], L); + D = Fns::invertSym33(Fns::multiplySym(&static_cast(*a.S), IS) + L*isf[i]); + + Fns::multiply(DSI, IS, D); + + r.D = Mat33V_From_PxMat33(D); + static_cast(r.DSI[0]) = DSI[0]; + static_cast(r.DSI[1]) = DSI[1]; + static_cast(r.DSI[2]) = DSI[2]; + + inertia[matrix.parent[i]] += Fns::translate(getParentOffset(jv), Fns::multiplySubtract(inertia[i], DSI, IS)); + } + + FsInertia& m = getRootInverseInertia(matrix); + m = FsInertia(Fns::invertInertia(inertia[0])); +} + +// no need to compile this ecxcept for verification, and it consumes huge amounts of stack space +void PxcFsComputeJointLoadsScalar(const FsData& matrix, + const FsInertia*PX_RESTRICT baseInertia, + Mat33V*PX_RESTRICT load, + const PxReal*PX_RESTRICT isf, + PxU32 linkCount, + PxU32 maxIterations) +{ + typedef ArticulationFnsScalar Fns; + + // the childward S + SpInertia leafwardInertia[DY_ARTICULATION_MAX_SIZE]; + SpInertia rootwardInertia[DY_ARTICULATION_MAX_SIZE]; + SpInertia inertia[DY_ARTICULATION_MAX_SIZE]; + SpInertia contribToParent[DY_ARTICULATION_MAX_SIZE]; + + // total articulated inertia assuming the articulation is rooted here + + const FsRow* row = getFsRows(matrix); + const FsRowAux* aux = getAux(matrix); + const FsJointVectors* jointVectors = getJointVectors(matrix); + + PX_UNUSED(row); + + PxMat33 load_[DY_ARTICULATION_MAX_SIZE]; + + for(PxU32 iter=0;iter1;) + { + const FsJointVectors& j = jointVectors[i]; + + leafwardInertia[i] = inertia[i]; + contribToParent[i] = Fns::propagate(inertia[i], &static_cast(*aux[i].S), load_[i], isf[i]); + inertia[matrix.parent[i]] += Fns::translate((PxVec3&)j.parentOffset, contribToParent[i]); + } + + for(PxU32 i=1;i(*aux[i].S), load_[i], isf[i]); + } + + for(PxU32 i=1;i(*aux[i].S)); + PX_ASSERT(load_[i][0].isFinite() && load_[i][1].isFinite() && load_[2][i].isFinite()); + } + } + for(PxU32 i=1;i(state.deferredZ) += Z; + state.dirty |= matrix.row[linkID].pathToRoot; +} + +Cm::SpatialVector PxcFsGetVelocity(const FsData& matrix, + PxU32 linkID) +{ + // find the dirty node on the path (including the root) with the lowest index + ArticulationBitField toUpdate = matrix.row[linkID].pathToRoot & state.dirty; + + if(toUpdate) + { + ArticulationBitField ignoreNodes = (toUpdate & (0-toUpdate))-1; + ArticulationBitField path = matrix.row[linkID].pathToRoot & ~ignoreNodes, p = path; + ArticulationBitField newDirty = 0; + + Cm::SpatialVector dV = Cm::SpatialVector::zero(); + if(p & 1) + { + dV = getRootDeltaV(matrix, -deferredZ(state)); + + velocityRef(state, 0) += dV; + for(ArticulationBitField defer = matrix.row[0].children & ~path; defer; defer &= (defer-1)) + deferredVelRef(state, ArticulationLowestSetBit(defer)) += dV; + + deferredZRef(state) = Cm::SpatialVector::zero(); + newDirty = matrix.row[0].children; + p--; + } + + for(; p; p &= (p-1)) + { + PxU32 i = ArticulationLowestSetBit(p); + + dV = propagateVelocity(matrix.row[i], deferredSZ(state,i), dV + state.deferredVel[i], matrix.aux[i]); + + velocityRef(state,i) += dV; + for(ArticulationBitField defer = matrix.row[i].children & ~path; defer; defer &= (defer-1)) + deferredVelRef(state,ArticulationLowestSetBit(defer)) += dV; + + newDirty |= matrix.row[i].children; + deferredVelRef(state,i) = Cm::SpatialVector::zero(); + deferredSZRef(state,i) = PxVec3(0); + } + + state.dirty = (state.dirty | newDirty)&~path; + } +#if DY_ARTICULATION_DEBUG_VERIFY + Cm::SpatialVector v = state.velocity[linkID]; + Cm::SpatialVector rv = state.refVelocity[linkID]; + PX_ASSERT((v-rv).magnitude()<1e-4f * rv.magnitude()); +#endif + + return state.velocity[linkID]; +} + +void PxcFsFlushVelocity(const FsData& matrix) +{ + Cm::SpatialVector V = getRootDeltaV(matrix, -deferredZ(state)); + deferredZRef(state) = Cm::SpatialVector::zero(); + velocityRef(state,0) += V; + for(ArticulationBitField defer = matrix.row[0].children; defer; defer &= (defer-1)) + deferredVelRef(state,ArticulationLowestSetBit(defer)) += V; + + for(PxU32 i = 1; i Fns; + + Cm::SpatialVectorV IS[3]; + PxMat33 D; + + FsRow* rows = getFsRows(matrix); + const FsRowAux* aux = getAux(matrix); + const FsJointVectors* jointVectors = getJointVectors(matrix); + + FsInertia *inertia = allocator.alloc(matrix.linkCount); + PxMemCopy(inertia, baseInertia, matrix.linkCount*sizeof(FsInertia)); + + for(PxU32 i=matrix.linkCount; --i>0;) + { + FsRow& r = rows[i]; + const FsRowAux& a = aux[i]; + const FsJointVectors& jv = jointVectors[i]; + + Mat33V m = Fns::computeSIS(inertia[i], a.S, IS); + FloatV f = FLoad(isf[i]); + + Mat33V D = Fns::invertSym33(Mat33V(V3ScaleAdd(load[i].col0, f, m.col0), + V3ScaleAdd(load[i].col1, f, m.col1), + V3ScaleAdd(load[i].col2, f, m.col2))); + r.D = D; + + inertia[matrix.parent[i]] = Fns::addInertia(inertia[matrix.parent[i]], + Fns::translateInertia(jv.parentOffset, Fns::multiplySubtract(inertia[i], D, IS, r.DSI))); + } + + getRootInverseInertia(matrix) = Fns::invertInertia(inertia[0]); +} + +void PxcLtbSolve(const FsData& m, + Vec3V* c, // rhs error to solve for + Cm::SpatialVector* y) // velocity delta output +{ + typedef ArticulationFnsScalar Fns; + + PxVec4* b = reinterpret_cast(c); + const LtbRow* rows = getLtbRows(m); + PxMemZero(y, m.linkCount*sizeof(Cm::SpatialVector)); + + for(PxU32 i=m.linkCount;i-->1;) + { + PxU32 p = m.parent[i]; + const LtbRow& r = rows[i]; + b[i] -= PxVec4(Fns::axisDot(&static_cast(*r.j1), y[i]),0); + y[p] -= Fns::axisMultiply(&static_cast(*r.j0), b[i].getXYZ()); + } + + y[0] = Fns::multiply(rows[0].inertia,y[0]); + + for(PxU32 i=1; i(*r.j0), y[p]); + y[i] = Fns::multiply(r.inertia, y[i]) - Fns::axisMultiply(&static_cast(*r.j1), t); + } +} + + +#endif + + +#if DY_ARTICULATION_DEBUG_VERIFY +void PxcLtbFactorScalar(FsData& m) +{ + typedef ArticulationFnsScalar Fns; + LtbRow* rows = getLtbRows(m); + + SpInertia inertia[DY_ARTICULATION_MAX_SIZE]; + for(PxU32 i=0;i0;) + { + LtbRow& b = rows[i]; + inertia[i] = Fns::invertInertia(inertia[i]); + PxU32 p = m.parent[i]; + + Cm::SpatialVector* j0 = &reinterpret_cast(*b.j0), + * j1 = &reinterpret_cast(*b.j1); + + Fns::multiply(j, inertia[i], j1); + PxMat33 jResponse = Fns::invertSym33(-Fns::multiplySym(j, j1)); + j1[0] = j[0]; j1[1] = j[1]; j1[2] = j[2]; + + b.jResponse = Mat33V_From_PxMat33(jResponse); + Fns::multiply(j, j0, jResponse); + inertia[p] = Fns::multiplySubtract(inertia[p], j, j0); + j0[0] = j[0]; j0[1] = j[1]; j0[2] = j[2]; + } + + rows[0].inertia = Fns::invertInertia(inertia[0]); + for(PxU32 i=1;i0;) + { + FsRow& r = rows[i]; + const FsRowAux& a = aux[i]; + const FsJointVectors& jv = jointVectors[i]; + + Fns::multiply(IS, inertia[i], &reinterpret_cast(*a.S)); + + PX_ALIGN(16, PxMat33) L; + PxMat33_From_Mat33V(load[i], L); + D = Fns::invertSym33(Fns::multiplySym(&reinterpret_cast(*a.S), IS) + L*isf[i]); + + Fns::multiply(DSI, IS, D); + + r.D = Mat33V_From_PxMat33(D); + reinterpret_cast(r.DSI[0]) = DSI[0]; + reinterpret_cast(r.DSI[1]) = DSI[1]; + reinterpret_cast(r.DSI[2]) = DSI[2]; + + inertia[matrix.parent[i]] += Fns::translate(getParentOffset(jv), Fns::multiplySubtract(inertia[i], DSI, IS)); + } + + FsInertia& m = getRootInverseInertia(matrix); + m = FsInertia(Fns::invertInertia(inertia[0])); +} + +// no need to compile this ecxcept for verification, and it consumes huge amounts of stack space +void PxcFsComputeJointLoadsScalar(const FsData& matrix, + const FsInertia*PX_RESTRICT baseInertia, + Mat33V*PX_RESTRICT load, + const PxReal*PX_RESTRICT isf, + PxU32 linkCount, + PxU32 maxIterations) +{ + typedef ArticulationFnsScalar Fns; + + // the childward S + SpInertia leafwardInertia[DY_ARTICULATION_MAX_SIZE]; + SpInertia rootwardInertia[DY_ARTICULATION_MAX_SIZE]; + SpInertia inertia[DY_ARTICULATION_MAX_SIZE]; + SpInertia contribToParent[DY_ARTICULATION_MAX_SIZE]; + + // total articulated inertia assuming the articulation is rooted here + + const FsRow* row = getFsRows(matrix); + const FsRowAux* aux = getAux(matrix); + const FsJointVectors* jointVectors = getJointVectors(matrix); + + PX_UNUSED(row); + + PxMat33 load_[DY_ARTICULATION_MAX_SIZE]; + + for(PxU32 iter=0;iter1;) + { + const FsJointVectors& j = jointVectors[i]; + + leafwardInertia[i] = inertia[i]; + contribToParent[i] = Fns::propagate(inertia[i], &reinterpret_cast(*aux[i].S), load_[i], isf[i]); + inertia[matrix.parent[i]] += Fns::translate((PxVec3&)j.parentOffset, contribToParent[i]); + } + + for(PxU32 i=1;i(*aux[i].S), load_[i], isf[i]); + } + + for(PxU32 i=1;i(*aux[i].S)); + PX_ASSERT(load_[i][0].isFinite() && load_[i][1].isFinite() && load_[2][i].isFinite()); + } + } + for(PxU32 i=1;i