165 lines
6.7 KiB
C++
165 lines
6.7 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-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 "ExtJoint.h"
|
||
|
|
|
||
|
|
#include "omnipvd/ExtOmniPvdSetData.h"
|
||
|
|
|
||
|
|
using namespace physx;
|
||
|
|
using namespace Ext;
|
||
|
|
|
||
|
|
// PX_SERIALIZATION
|
||
|
|
PxConstraint* physx::resolveConstraintPtr(PxDeserializationContext& v, PxConstraint* old, PxConstraintConnector* connector, PxConstraintShaderTable &shaders)
|
||
|
|
{
|
||
|
|
v.translatePxBase(old);
|
||
|
|
PxConstraint* new_nx = static_cast<PxConstraint*>(old);
|
||
|
|
new_nx->setConstraintFunctions(*connector, shaders);
|
||
|
|
return new_nx;
|
||
|
|
}
|
||
|
|
//~PX_SERIALIZATION
|
||
|
|
|
||
|
|
static void normalToTangents(const PxVec3& n, PxVec3& t1, PxVec3& t2)
|
||
|
|
{
|
||
|
|
const PxReal m_sqrt1_2 = PxReal(0.7071067811865475244008443621048490);
|
||
|
|
if(fabsf(n.z) > m_sqrt1_2)
|
||
|
|
{
|
||
|
|
const PxReal a = n.y*n.y + n.z*n.z;
|
||
|
|
const PxReal k = PxReal(1.0)/PxSqrt(a);
|
||
|
|
t1 = PxVec3(0,-n.z*k,n.y*k);
|
||
|
|
t2 = PxVec3(a*k,-n.x*t1.z,n.x*t1.y);
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
const PxReal a = n.x*n.x + n.y*n.y;
|
||
|
|
const PxReal k = PxReal(1.0)/PxSqrt(a);
|
||
|
|
t1 = PxVec3(-n.y*k,n.x*k,0);
|
||
|
|
t2 = PxVec3(-n.z*t1.y,n.z*t1.x,a*k);
|
||
|
|
}
|
||
|
|
t1.normalize();
|
||
|
|
t2.normalize();
|
||
|
|
}
|
||
|
|
|
||
|
|
void PxSetJointGlobalFrame(PxJoint& joint, const PxVec3* wsAnchor, const PxVec3* axisIn)
|
||
|
|
{
|
||
|
|
PxRigidActor* actors[2];
|
||
|
|
joint.getActors(actors[0], actors[1]);
|
||
|
|
|
||
|
|
PxTransform localPose[2];
|
||
|
|
for(PxU32 i=0; i<2; i++)
|
||
|
|
localPose[i] = PxTransform(PxIdentity);
|
||
|
|
|
||
|
|
// 1) global anchor
|
||
|
|
if(wsAnchor)
|
||
|
|
{
|
||
|
|
//transform anchorPoint to local space
|
||
|
|
for(PxU32 i=0; i<2; i++)
|
||
|
|
localPose[i].p = actors[i] ? actors[i]->getGlobalPose().transformInv(*wsAnchor) : *wsAnchor;
|
||
|
|
}
|
||
|
|
|
||
|
|
// 2) global axis
|
||
|
|
if(axisIn)
|
||
|
|
{
|
||
|
|
PxVec3 localAxis[2], localNormal[2];
|
||
|
|
|
||
|
|
//find 2 orthogonal vectors.
|
||
|
|
//gotta do this in world space, if we choose them
|
||
|
|
//separately in local space they won't match up in worldspace.
|
||
|
|
PxVec3 axisw = *axisIn;
|
||
|
|
axisw.normalize();
|
||
|
|
|
||
|
|
PxVec3 normalw, binormalw;
|
||
|
|
::normalToTangents(axisw, binormalw, normalw);
|
||
|
|
//because axis is supposed to be the Z axis of a frame with the other two being X and Y, we need to negate
|
||
|
|
//Y to make the frame right handed. Note that the above call makes a right handed frame if we pass X --> Y,Z, so
|
||
|
|
//it need not be changed.
|
||
|
|
|
||
|
|
for(PxU32 i=0; i<2; i++)
|
||
|
|
{
|
||
|
|
if(actors[i])
|
||
|
|
{
|
||
|
|
const PxTransform& m = actors[i]->getGlobalPose();
|
||
|
|
PxMat33 mM(m.q);
|
||
|
|
localAxis[i] = mM.transformTranspose(axisw);
|
||
|
|
localNormal[i] = mM.transformTranspose(normalw);
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
localAxis[i] = axisw;
|
||
|
|
localNormal[i] = normalw;
|
||
|
|
}
|
||
|
|
|
||
|
|
PxMat33 rot(localAxis[i], localNormal[i], localAxis[i].cross(localNormal[i]));
|
||
|
|
|
||
|
|
localPose[i].q = PxQuat(rot);
|
||
|
|
localPose[i].q.normalize();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
for(PxU32 i=0; i<2; i++)
|
||
|
|
joint.setLocalPose(static_cast<PxJointActorIndex::Enum>( i ), localPose[i]);
|
||
|
|
}
|
||
|
|
|
||
|
|
#if PX_SUPPORT_OMNI_PVD
|
||
|
|
|
||
|
|
void physx::Ext::omniPvdSetBaseJointParams(const PxJoint& joint, PxJointConcreteType::Enum cType)
|
||
|
|
{
|
||
|
|
OMNI_PVD_WRITE_SCOPE_BEGIN(pvdWriter, pvdRegData)
|
||
|
|
|
||
|
|
const PxJoint& j = static_cast<const PxJoint&>(joint);
|
||
|
|
PxRigidActor* actors[2]; j.getActors(actors[0], actors[1]);
|
||
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxJoint, actor0, j, actors[0])
|
||
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxJoint, actor1, j, actors[1])
|
||
|
|
|
||
|
|
PxConstraint* constraint = j.getConstraint();
|
||
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxJoint, constraint, j, constraint)
|
||
|
|
|
||
|
|
PxTransform actor0LocalPose = j.getLocalPose(PxJointActorIndex::eACTOR0);
|
||
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxJoint, actor0LocalPose, j, actor0LocalPose)
|
||
|
|
PxTransform actor1LocalPose = j.getLocalPose(PxJointActorIndex::eACTOR1);
|
||
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxJoint, actor1LocalPose, j, actor1LocalPose)
|
||
|
|
PxReal breakForce, breakTorque; j.getBreakForce(breakForce, breakTorque);
|
||
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxJoint, breakForce, j, breakForce)
|
||
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxJoint, breakTorque, j, breakTorque)
|
||
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxJoint, constraintFlags, j, j.getConstraintFlags())
|
||
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxJoint, invMassScale0, j, j.getInvMassScale0())
|
||
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxJoint, invInertiaScale0, j, j.getInvInertiaScale0())
|
||
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxJoint, invMassScale1, j, j.getInvMassScale1())
|
||
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxJoint, invInertiaScale1, j, j.getInvInertiaScale1())
|
||
|
|
const char* name = j.getName() ? j.getName() : "";
|
||
|
|
PxU32 nameLen = PxU32(strlen(name)) + 1;
|
||
|
|
OMNI_PVD_SET_ARRAY_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxJoint, name, j, name, nameLen)
|
||
|
|
const char* typeName = j.getConcreteTypeName();
|
||
|
|
PxU32 typeNameLen = PxU32(strlen(typeName)) + 1;
|
||
|
|
OMNI_PVD_SET_ARRAY_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxJoint, concreteTypeName, j, typeName, typeNameLen)
|
||
|
|
OMNI_PVD_SET_EXPLICIT(pvdWriter, pvdRegData, OMNI_PVD_CONTEXT_HANDLE, PxJoint, type, j, cType)
|
||
|
|
|
||
|
|
OMNI_PVD_WRITE_SCOPE_END
|
||
|
|
}
|
||
|
|
|
||
|
|
#endif
|