diff options
Diffstat (limited to 'Runtime/Dynamics/ConfigurableJoint.cpp')
| -rw-r--r-- | Runtime/Dynamics/ConfigurableJoint.cpp | 479 |
1 files changed, 479 insertions, 0 deletions
diff --git a/Runtime/Dynamics/ConfigurableJoint.cpp b/Runtime/Dynamics/ConfigurableJoint.cpp new file mode 100644 index 0000000..b036e24 --- /dev/null +++ b/Runtime/Dynamics/ConfigurableJoint.cpp @@ -0,0 +1,479 @@ +#include "UnityPrefix.h" +#if ENABLE_PHYSICS +#include "ConfigurableJoint.h" +#include "Runtime/Graphics/Transform.h" +#include "Runtime/Serialize/TransferFunctions/SerializeTransfer.h" +#include "Runtime/Serialize/TransferFunctions/TransferNameConversions.h" +#include "PhysicsManager.h" +#include "Runtime/Utilities/Utility.h" +#include "Runtime/Misc/BuildSettings.h" +#include "NxWrapperUtility.h" + +#include "External/PhysX/builds/SDKs/Physics/include/NxPhysics.h" + +using namespace std; + +namespace Unity +{ + +#define GET_JOINT() static_cast<NxD6Joint*> (m_Joint) + + +/* +- We awake the hingejoint only once. (AwakeFromLoad) + At this point we setup the axes. They are never changed afterwards + -> The perfect solution remembers the old position/rotation of the rigid bodies. + Then when changing axis/anchor is changed it generates axes that are rleative to the old position/rotation state! +*/ + +void ConfigurableJoint::InitializeClass () +{ + #if UNITY_EDITOR + // Only introduced during 2.0 alpha + RegisterAllowNameConversion (ConfigurableJoint::GetClassStringStatic(), "m_ConfigureInWorldSpace", "m_ConfiguredInWorldSpace"); + #endif +} + + + +ConfigurableJoint::ConfigurableJoint (MemLabelId label, ObjectCreationMode mode) +: Super(label, mode) +{ +} + +ConfigurableJoint::~ConfigurableJoint () +{ +} + +void ConfigurableJoint::CalculateGlobalHingeSpace (Vector3f& globalAnchor, Vector3f& globalAxis, Vector3f& globalNormal) const +{ + const Transform& transform = GetComponent (Transform); + + Vector3f localAxis = m_Axis; + if (SqrMagnitude (localAxis) < Vector3f::epsilon) + localAxis = Vector3f (1.0F, 0.0F, 0.0F); + Vector3f localNormal = m_SecondaryAxis; + + OrthoNormalize (&localAxis, &localNormal); + + globalAnchor = transform.TransformPoint (m_Anchor); + //Vector3f globalRigidbodyPos = transform.GetPosition (); + + if (m_ConfiguredInWorldSpace) + { + globalAxis = localAxis; + globalNormal = localNormal; + } + else + { + globalAxis = transform.TransformDirection (localAxis); + globalNormal = transform.TransformDirection (localNormal); + } +// Matrix3x3f m; +// m.SetOrthoNormalBasisInverse(globalAxis, globalNormal, Cross (globalAxis, globalNormal)); +// m.SetOrthoNormalBasisInverse(Vector3f::xAxis, globalNormal, Cross (globalAxis, globalNormal)); +// m.SetOrthoNormalBasisInverse(globalNormal, Cross (globalAxis, globalNormal), globalAxis); +// OrthoNormalize(m); +// Quaternionf q; +// MatrixToQuaternion(m, q); +// m_ConfigurationSpace = q * Inverse(transform.GetRotation ()); +} + +void ConfigurableJoint::Reset () +{ + Super::Reset(); + + m_XMotion = NX_D6JOINT_MOTION_FREE; + m_YMotion = NX_D6JOINT_MOTION_FREE; + m_ZMotion = NX_D6JOINT_MOTION_FREE; + + m_AngularXMotion = NX_D6JOINT_MOTION_FREE; + m_AngularYMotion = NX_D6JOINT_MOTION_FREE; + m_AngularZMotion = NX_D6JOINT_MOTION_FREE; + + InitSoftJointLimit(m_LinearLimit); + InitSoftJointLimit(m_LowAngularXLimit); + InitSoftJointLimit(m_HighAngularXLimit); + InitSoftJointLimit(m_AngularYLimit); + InitSoftJointLimit(m_AngularZLimit); + + InitJointDrive(m_XDrive); + InitJointDrive(m_YDrive); + InitJointDrive(m_ZDrive); + InitJointDrive(m_AngularXDrive); + InitJointDrive(m_AngularYZDrive); + InitJointDrive(m_SlerpDrive); + + m_ProjectionMode = 0; + m_ProjectionDistance = 0.1F; + m_ProjectionAngle = 5.0F; +// m_GearRatio = 1.0F; + m_RotationDriveMode = 0; +// m_UseGear = false; + m_TargetAngularVelocity = m_TargetVelocity = m_TargetPosition = Vector3f(0.0F, 0.0F, 0.0F); + m_TargetRotation = Quaternionf::identity(); + m_SecondaryAxis = Vector3f::yAxis; + m_ConfiguredInWorldSpace = false; + m_SwapBodies = false; +} + +void ConfigurableJoint::CheckConsistency () +{ + Super::CheckConsistency(); +} + +void ConfigurableJoint::SetXMotion (int motion) +{ + m_XMotion = motion; + ApplyKeepConfigurationSpace(); +} + +void ConfigurableJoint::SetYMotion (int motion) +{ + m_YMotion = motion; + ApplyKeepConfigurationSpace(); +} + +void ConfigurableJoint::SetAngularXMotion (int motion) +{ + m_AngularXMotion = motion; + ApplyKeepConfigurationSpace(); +} + +void ConfigurableJoint::SetAngularYMotion (int motion) +{ + m_AngularYMotion = motion; + ApplyKeepConfigurationSpace(); +} + +void ConfigurableJoint::SetAngularZMotion (int motion) +{ + m_AngularZMotion = motion; + ApplyKeepConfigurationSpace(); +} + +void ConfigurableJoint::SetZMotion (int motion) +{ + m_ZMotion = motion; + ApplyKeepConfigurationSpace(); +} + +void ConfigurableJoint::SetLinearLimit (const SoftJointLimit& limit) +{ + m_LinearLimit = limit; + ApplyKeepConfigurationSpace(); +} + +void ConfigurableJoint::SetAngularZLimit (const SoftJointLimit& limit) +{ + m_AngularZLimit = limit; + ApplyKeepConfigurationSpace(); +} + +void ConfigurableJoint::SetAngularYLimit (const SoftJointLimit& limit) +{ + m_AngularYLimit = limit; + ApplyKeepConfigurationSpace(); +} + +void ConfigurableJoint::SetLowAngularXLimit (const SoftJointLimit& limit) +{ + m_LowAngularXLimit = limit; + ApplyKeepConfigurationSpace(); +} + +void ConfigurableJoint::SetHighAngularXLimit (const SoftJointLimit& limit) +{ + m_HighAngularXLimit = limit; + ApplyKeepConfigurationSpace(); +} + +void ConfigurableJoint::SetXDrive (const JointDrive& drive) +{ + m_XDrive = drive; + ApplyKeepConfigurationSpace(); +} + +void ConfigurableJoint::SetYDrive (const JointDrive& drive) +{ + m_YDrive = drive; + ApplyKeepConfigurationSpace(); +} + +void ConfigurableJoint::SetZDrive (const JointDrive& drive) +{ + m_ZDrive = drive; + ApplyKeepConfigurationSpace(); +} + +void ConfigurableJoint::SetAngularXDrive (const JointDrive& drive) +{ + m_AngularXDrive = drive; + ApplyKeepConfigurationSpace(); +} + +void ConfigurableJoint::SetAngularYZDrive (const JointDrive& drive) +{ + m_AngularYZDrive = drive; + ApplyKeepConfigurationSpace(); +} + +void ConfigurableJoint::SetSlerpDrive (const JointDrive& drive) +{ + m_SlerpDrive = drive; + ApplyKeepConfigurationSpace(); +} + +void ConfigurableJoint::SetRotationDriveMode (int mode) +{ + m_RotationDriveMode = mode; + ApplyKeepConfigurationSpace(); +} + +void ConfigurableJoint::SetProjectionMode (int mode) +{ + m_ProjectionMode = mode; + ApplyKeepConfigurationSpace(); +} + +void ConfigurableJoint::SetProjectionDistance (float dist) +{ + m_ProjectionDistance = dist; + ApplyKeepConfigurationSpace(); +} + +void ConfigurableJoint::SetProjectionAngle (float angle) +{ + m_ProjectionAngle = angle; + ApplyKeepConfigurationSpace(); +} + +void ConfigurableJoint::SetConfiguredInWorldSpace (bool c) +{ + m_ConfiguredInWorldSpace = c; + ApplyRebuildConfigurationSpace(); +} + +void ConfigurableJoint::SetSwapBodies (bool c) +{ + m_SwapBodies = c; + ApplyRebuildConfigurationSpace(); +} + +void ConfigurableJoint::SetTargetPosition (const Vector3f& pos) +{ + m_TargetPosition = pos; + if (m_Joint) + GET_JOINT()->setDrivePosition(Vec3ToNx(pos)); +} + +void ConfigurableJoint::SetTargetRotation (const Quaternionf& rotation) +{ + m_TargetRotation = rotation; + if (m_Joint) + GET_JOINT()->setDriveOrientation((const NxQuat&)rotation); +} + +void ConfigurableJoint::SetTargetVelocity (const Vector3f& vel) +{ + m_TargetVelocity = vel; + if (m_Joint) + GET_JOINT()->setDriveLinearVelocity(Vec3ToNx(vel)); +} + +void ConfigurableJoint::SetTargetAngularVelocity (const Vector3f& angular) +{ + m_TargetAngularVelocity = angular; + if (m_Joint) + GET_JOINT()->setDriveAngularVelocity(Vec3ToNx(angular)); +} + +/* +- When the rigid body which is attached with the hinge joint is activated after the joint is loaded + It will not be connected with the joint! +*/ + +template<class TransferFunction> +void ConfigurableJoint::Transfer (TransferFunction& transfer) +{ + JointTransferPre (transfer); + TRANSFER (m_SecondaryAxis); + + TRANSFER(m_XMotion); + TRANSFER(m_YMotion); + TRANSFER(m_ZMotion); + TRANSFER(m_AngularXMotion); + TRANSFER(m_AngularYMotion); + TRANSFER(m_AngularZMotion); + + TRANSFER(m_LinearLimit); + TRANSFER(m_LowAngularXLimit); + TRANSFER(m_HighAngularXLimit); + TRANSFER(m_AngularYLimit); + TRANSFER(m_AngularZLimit); + + TRANSFER(m_TargetPosition); + TRANSFER(m_TargetVelocity); + + TRANSFER(m_XDrive); + TRANSFER(m_YDrive); + TRANSFER(m_ZDrive); + + TRANSFER(m_TargetRotation); + TRANSFER(m_TargetAngularVelocity); + + TRANSFER(m_RotationDriveMode); + + TRANSFER(m_AngularXDrive); + TRANSFER(m_AngularYZDrive); + TRANSFER(m_SlerpDrive); + + TRANSFER(m_ProjectionMode); + TRANSFER(m_ProjectionDistance); + TRANSFER(m_ProjectionAngle); + + TRANSFER(m_ConfiguredInWorldSpace); + TRANSFER(m_SwapBodies); + transfer.Align(); + + JointTransferPost (transfer); +} + +void ConfigurableJoint::ApplyKeepConfigurationSpace() +{ + SetDirty(); + if (m_Joint) + { + // Joint has NX_JS_UNBOUND state, when it's created, but physics aren't simulated yet. + // For ex., if you create ConfigurableJoint from a script, right after creation it will have such state. + if (m_Joint->getState () == NX_JS_SIMULATING || m_Joint->getState () == NX_JS_UNBOUND) + { + NxD6JointDesc desc; + GET_JOINT()->saveToDesc (desc); + SetupD6Desc(desc); + GET_JOINT()->loadFromDesc (desc); + } + } +} + +void ConfigurableJoint::ApplyRebuildConfigurationSpace() +{ + SetDirty(); + if (m_Joint) + { + GetDynamicsScene ().releaseJoint (*m_Joint); + m_Joint = NULL; + } + Create(); +} + + +void ConfigurableJoint::SetupD6Desc (NxD6JointDesc& desc) +{ + desc.xMotion = (NxD6JointMotion)m_XMotion; + desc.yMotion = (NxD6JointMotion)m_YMotion; + desc.zMotion = (NxD6JointMotion)m_ZMotion; + + desc.swing1Motion = (NxD6JointMotion)m_AngularYMotion; + desc.swing2Motion = (NxD6JointMotion)m_AngularZMotion; + desc.twistMotion = (NxD6JointMotion)m_AngularXMotion; + + ConvertSoftLimitLinear(m_LinearLimit, desc.linearLimit); + ConvertSoftLimit(m_AngularYLimit, desc.swing1Limit); + ConvertSoftLimit(m_AngularZLimit, desc.swing2Limit); + ConvertSoftLimit(m_LowAngularXLimit, desc.twistLimit.low); + ConvertSoftLimit(m_HighAngularXLimit, desc.twistLimit.high); + + if (desc.twistLimit.low.value > desc.twistLimit.high.value) + swap (desc.twistLimit.low, desc.twistLimit.high); + + ConvertDrive(m_XDrive, desc.xDrive); + ConvertDrive(m_YDrive, desc.yDrive); + ConvertDrive(m_ZDrive, desc.zDrive); + + ConvertDrive(m_AngularXDrive, desc.twistDrive); + ConvertDrive(m_AngularYZDrive, desc.swingDrive); + ConvertDrive(m_SlerpDrive, desc.slerpDrive); + + desc.projectionMode = (NxJointProjectionMode)m_ProjectionMode; + desc.projectionDistance = m_ProjectionDistance; + desc.projectionAngle = Deg2Rad(m_ProjectionAngle); +// desc.gearRatio = m_GearRatio; + + desc.flags = 0; + if (m_RotationDriveMode) + desc.flags |= NX_D6JOINT_SLERP_DRIVE; + +// if (m_UseGear) +// desc.flags |= NX_D6JOINT_GEAR_ENABLED; + + desc.driveAngularVelocity = Vec3ToNx(m_TargetAngularVelocity); + desc.driveOrientation = (NxQuat&)m_TargetRotation; + desc.driveLinearVelocity = Vec3ToNx(m_TargetVelocity); + desc.drivePosition = Vec3ToNx(m_TargetPosition); +} + +void ConfigurableJoint::Create () +{ + AssertIf (!IsActive ()); + + NxD6JointDesc desc; + + if (m_Joint && m_Joint->getState () == NX_JS_SIMULATING) + GET_JOINT()->saveToDesc (desc); + + SetupD6Desc (desc); + FinalizeCreateD6 (desc); +} + +void ConfigurableJoint::FinalizeCreateD6 (NxD6JointDesc& desc) +{ + bool swapBodies = m_SwapBodies || (m_ConfiguredInWorldSpace && !IS_CONTENT_NEWER_OR_SAME(kUnityVersion3_4_a1)); + FinalizeCreateImpl (desc, swapBodies); + if (swapBodies) + { + swap(desc.localNormal[0], desc.localNormal[1]); + swap(desc.localAxis[0], desc.localAxis[1]); + swap(desc.localAnchor[0], desc.localAnchor[1]); + } + + if (GET_JOINT ()) + { + GET_JOINT ()->loadFromDesc (desc); + + // When actors change, we can't use loadFromDesc on the same joint. + // Thus recreate the joint + if (m_Joint && m_Joint->getState () == NX_JS_BROKEN) + { + GetDynamicsScene ().releaseJoint (*m_Joint); + m_Joint = GetDynamicsScene ().createJoint (desc); + } + } + else + m_Joint = GetDynamicsScene ().createJoint (desc); +} + + +void ConfigurableJoint::SetSecondaryAxis (const Vector3f& axis) +{ + SetDirty (); + m_SecondaryAxis = axis; + if (IsActive () && GET_JOINT()) + { + NxD6JointDesc desc; + AssertIf (m_Joint->getState () == NX_JS_BROKEN); + GET_JOINT()->saveToDesc (desc); + + SetupAxes (desc, kChangeAxis); + GET_JOINT()->loadFromDesc (desc); + AssertIf (m_Joint->getState () == NX_JS_BROKEN); + } +} + +IMPLEMENT_AXIS_ANCHOR(ConfigurableJoint,NxD6JointDesc) + +} + +IMPLEMENT_CLASS_HAS_INIT (ConfigurableJoint) +IMPLEMENT_OBJECT_SERIALIZE (ConfigurableJoint) +#endif //ENABLE_PHSYICS |