void PezUpdate(unsigned int elapsedMicroseconds) { const float RadiansPerMicrosecond = 0.0000005f; static float Theta = 0; Theta += elapsedMicroseconds * RadiansPerMicrosecond; Vector3 offset = V3MakeFromElems(0, 0, 0); Matrix4 model = M4MakeRotationZ(Theta); model = M4Mul(M4MakeTranslation(offset), model); model = M4Mul(model, M4MakeTranslation(V3Neg(offset))); Point3 eyePosition = P3MakeFromElems(0, 10, 0); Point3 targetPosition = P3MakeFromElems(0, 0, 0); Vector3 upVector = V3MakeFromElems(0, 0, 1); Matrix4 view = M4MakeLookAt(eyePosition, targetPosition, upVector); ModelviewMatrix = M4Mul(view, model); }
void PxcArticulationHelper::getImpulseSelfResponse(const PxcFsData& matrix, PxU32 linkID0, const PxcSIMDSpatial& impulse0, PxcSIMDSpatial& deltaV0, PxU32 linkID1, const PxcSIMDSpatial& impulse1, PxcSIMDSpatial& deltaV1) { PX_ASSERT(linkID0 != linkID1); const PxcFsRow* rows = getFsRows(matrix); const PxcFsRowAux* aux = getAux(matrix); const PxcFsJointVectors* jointVectors = getJointVectors(matrix); PX_UNUSED(aux); PxcSIMDSpatial& dV0 = deltaV0, & dV1 = deltaV1; // standard case: parent-child limit if(matrix.parent[linkID1] == linkID0) { const PxcFsRow& r = rows[linkID1]; const PxcFsJointVectors& j = jointVectors[linkID1]; Vec3V lZ = V3Neg(impulse1.linear), aZ = V3Neg(impulse1.angular); Vec3V sz = V3Add(aZ, V3Cross(lZ, j.jointOffset)); lZ = V3Sub(lZ, V3ScaleAdd(r.DSI[0].linear, V3GetX(sz), V3ScaleAdd(r.DSI[1].linear, V3GetY(sz), V3Scale(r.DSI[2].linear, V3GetZ(sz))))); aZ = V3Sub(aZ, V3ScaleAdd(r.DSI[0].angular, V3GetX(sz), V3ScaleAdd(r.DSI[1].angular, V3GetY(sz), V3Scale(r.DSI[2].angular, V3GetZ(sz))))); aZ = V3Add(aZ, V3Cross(j.parentOffset, lZ)); lZ = V3Sub(impulse0.linear, lZ); aZ = V3Sub(impulse0.angular, aZ); dV0 = getImpulseResponseSimd(matrix, linkID0, lZ, aZ); Vec3V aV = dV0.angular; Vec3V lV = V3Sub(dV0.linear, V3Cross(j.parentOffset, aV)); Vec3V n = V3Add(V3Merge(V3Dot(r.DSI[0].linear, lV), V3Dot(r.DSI[1].linear, lV), V3Dot(r.DSI[2].linear, lV)), V3Merge(V3Dot(r.DSI[0].angular, aV), V3Dot(r.DSI[1].angular, aV), V3Dot(r.DSI[2].angular, aV))); n = V3Add(n, M33MulV3(r.D, sz)); lV = V3Sub(lV, V3Cross(j.jointOffset, n)); aV = V3Sub(aV, n); dV1 = PxcSIMDSpatial(lV, aV); } else getImpulseResponseSlow(matrix, linkID0, impulse0, deltaV0, linkID1, impulse1, deltaV1); #if PXC_ARTICULATION_DEBUG_VERIFY PxcSIMDSpatial dV0_, dV1_; PxcFsGetImpulseSelfResponse(matrix, linkID0, impulse0, dV0_, linkID1, impulse1, dV1_); PX_ASSERT(almostEqual(dV0_, dV0, 1e-3f)); PX_ASSERT(almostEqual(dV1_, dV1, 1e-3f)); #endif }