protected override void OnUpdatePhysXMotionLockedAxisParameters()
{
base.OnUpdatePhysXMotionLockedAxisParameters();
if (nativeJoint != IntPtr.Zero)
{
PhysXNativeD6Joint.SetMotion(nativeJoint, PhysXD6Axis.X,
PhysX_MotionLockedAxisX ? PhysXD6Motion.Locked : PhysXD6Motion.Free);
PhysXNativeD6Joint.SetMotion(nativeJoint, PhysXD6Axis.Y,
PhysX_MotionLockedAxisY ? PhysXD6Motion.Locked : PhysXD6Motion.Free);
PhysXNativeD6Joint.SetMotion(nativeJoint, PhysXD6Axis.Z,
PhysX_MotionLockedAxisZ ? PhysXD6Motion.Locked : PhysXD6Motion.Free);
}
}
void UpdateLimits()
{
PhysXNativeD6Joint.SetMotion(nativeJoint, PhysXD6Axis.X, PhysXD6Motion.Free);
if (axis.LimitsEnabled)
{
if (Math.Abs(axis.LimitLow + axis.LimitHigh) > .001f)
{
Log.Warning("PhysXSliderJoint: Different limit values for Slider joint are not supported. " +
"Equal limits are supported only. Example: LimitLow = -3; LimitHigh = 3.");
}
else
{
PhysXNativeD6Joint.SetMotion(nativeJoint, PhysXD6Axis.X, PhysXD6Motion.Limited);
PhysXNativeD6Joint.SetLinearLimit(nativeJoint, axis.LimitHigh, PhysXGeneral.jointLimitContactDistance,
axis.LimitsRestitution, axis.LimitsSpring, axis.LimitsDamping);
}
//PhysXNativeD6Joint.SetLinearLimit( nativeJoint, ( globalAnchor - low ).Length(),
// PhysXGeneral.jointLimitContactDistance, axis.LimitsBounciness, axis.LimitsHardness );
//PhysXNativeD6Joint.SetLinearLimit( nativeJoint, diff / 2, PhysXGeneral.jointLimitContactDistance,
// axis.LimitsBounciness, axis.LimitsHardness );
}
}
void UpdatePhysXJoint()
{
bool needCreate = PushedToWorld && !Broken;
bool created = nativeJoint != IntPtr.Zero;
if (needCreate == created)
{
return;
}
if (needCreate)
{
PhysXBody physXBody0 = (PhysXBody)Body1;
PhysXBody physXBody1 = (PhysXBody)Body2;
if ((!physXBody0.Static || !physXBody1.Static) &&
(physXBody0.nativeBody != IntPtr.Zero && physXBody1.nativeBody != IntPtr.Zero))
{
Vec3 globalAnchor = (Body1.Position + Body2.Position) * .5f;
//float diff = axis.LimitHigh - axis.LimitLow;
//Vec3 low = globalAnchor + Axis.Direction * axis.LimitLow;
//Vec3 high = globalAnchor + Axis.Direction * axis.LimitHigh;
//if( axis.LimitsEnabled )
//{
// globalAnchor = ( low + high ) / 2;
// //float d = axis.LimitHigh + axis.LimitLow;
// //если low положительный?
// //globalAnchor += ( d / 2 ) * Axis.Direction;
//}
Quat globalAxisRotation = Quat.FromDirectionZAxisUp(Axis.Direction);
Vec3 localPosition0 = physXBody0.Rotation.GetInverse() * (globalAnchor - physXBody0.Position);
Quat localRotation0 = (physXBody0.Rotation.GetInverse() * globalAxisRotation).GetNormalize();
Vec3 localPosition1 = physXBody1.Rotation.GetInverse() * (globalAnchor - physXBody1.Position);
Quat localRotation1 = (physXBody1.Rotation.GetInverse() * globalAxisRotation).GetNormalize();
nativeJoint = PhysXNativeWrapper.PhysXNativeD6Joint.Create(
physXBody0.nativeBody, ref localPosition0, ref localRotation0,
physXBody1.nativeBody, ref localPosition1, ref localRotation1);
PhysXNativeD6Joint.SetMotion(nativeJoint, PhysXD6Axis.X, PhysXD6Motion.Free);
PhysXNativeD6Joint.SetMotion(nativeJoint, PhysXD6Axis.Y, PhysXD6Motion.Locked);
PhysXNativeD6Joint.SetMotion(nativeJoint, PhysXD6Axis.Z, PhysXD6Motion.Locked);
PhysXNativeD6Joint.SetMotion(nativeJoint, PhysXD6Axis.Twist, PhysXD6Motion.Locked);
PhysXNativeD6Joint.SetMotion(nativeJoint, PhysXD6Axis.Swing1, PhysXD6Motion.Locked);
PhysXNativeD6Joint.SetMotion(nativeJoint, PhysXD6Axis.Swing2, PhysXD6Motion.Locked);
UpdateLimits();
//Vec3 globalAnchor = ( Body1.Position + Body2.Position ) * .5f;
//Quat globalAxisRotation = Quat.FromDirectionZAxisUp( Axis.Direction );
//Vec3 localPosition0 = physXBody0.Rotation.GetInverse() * ( globalAnchor - physXBody0.Position );
//Quat localRotation0 = ( physXBody0.Rotation.GetInverse() * globalAxisRotation ).GetNormalize();
//Vec3 localPosition1 = physXBody1.Rotation.GetInverse() * ( globalAnchor - physXBody1.Position );
//Quat localRotation1 = ( physXBody1.Rotation.GetInverse() * globalAxisRotation ).GetNormalize();
//nativeJoint = PhysXNativeWrapper.PhysXNativeSliderJoint.Create(
// physXBody0.nativeBody, ref localPosition0, ref localRotation0,
// physXBody1.nativeBody, ref localPosition1, ref localRotation1 );
//if( axis.LimitsEnabled )
//{
// Log.Warning( "PhysXSliderJoint: Limits for Slider joint are not supported by the PhysX Physics System." );
// //PhysXNativeWrapper.PhysXNativeSliderJoint.SetLimit( nativeJoint, true, axis.LimitLow, axis.LimitHigh,
// // PhysXGeneral.jointLimitContactDistance, axis.LimitsBounciness, axis.LimitsHardness );
//}
if (ContactsEnabled)
{
PhysXNativeWrapper.PhysXJoint.SetCollisionEnable(nativeJoint, true);
}
UpdatePhysXBreakData();
if (Scene._EnableDebugVisualization)
{
SetVisualizationEnable(true);
}
}
}
else
{
DestroyPhysXJoint();
}
}
void UpdateLimits()
{
PhysXNativeD6Joint.SetMotion(nativeJoint, PhysXD6Axis.Twist, PhysXD6Motion.Locked);
PhysXNativeD6Joint.SetMotion(nativeJoint, PhysXD6Axis.Swing1, PhysXD6Motion.Free);
PhysXNativeD6Joint.SetMotion(nativeJoint, PhysXD6Axis.Swing2, PhysXD6Motion.Free);
if (axis1.LimitsEnabled || axis2.LimitsEnabled)
{
float yAngle = 0;
float zAngle = 0;
if (axis1.LimitsEnabled)
{
if (Math.Abs(axis1.LimitLow + axis1.LimitHigh) > .001f)
{
Log.Warning("PhysXUniversalJoint: Different limit values for Universal joint are not supported. " +
"Equal limits are supported only. Example: LimitLow = -10; LimitHigh = 10.");
}
else
{
PhysXNativeD6Joint.SetMotion(nativeJoint, PhysXD6Axis.Swing1, PhysXD6Motion.Limited);
yAngle = Math.Max(Math.Abs(axis1.LimitLow.InRadians()), Math.Abs(axis1.LimitHigh.InRadians()));
}
}
if (axis2.LimitsEnabled)
{
if (Math.Abs(axis2.LimitLow + axis2.LimitHigh) > .001f)
{
Log.Warning("PhysXUniversalJoint: Different limit values for Universal joint are not supported. " +
"Equal limits are supported only. Example: LimitLow = -10; LimitHigh = 10.");
}
else
{
PhysXNativeD6Joint.SetMotion(nativeJoint, PhysXD6Axis.Swing2, PhysXD6Motion.Limited);
zAngle = Math.Max(Math.Abs(axis2.LimitLow.InRadians()), Math.Abs(axis2.LimitHigh.InRadians()));
}
}
if (axis1.LimitsEnabled && axis2.LimitsEnabled)
{
if (Math.Abs(axis1.LimitsRestitution - axis2.LimitsRestitution) > .001f ||
Math.Abs(axis1.LimitsSpring - axis2.LimitsSpring) > .001f ||
Math.Abs(axis1.LimitsDamping - axis2.LimitsDamping) > .001f)
{
Log.Warning("PhysXUniversalJoint: Different limit material properties for axes are not supported. " +
"Set equal LimitsRestitution, LimitsSpring, LimitsDamping values for both axes.");
}
}
float swingRestitution;
float swingSpring;
float swingDamping;
if (axis1.LimitsEnabled)
{
swingRestitution = axis1.LimitsRestitution;
swingSpring = axis1.LimitsSpring;
swingDamping = axis1.LimitsDamping;
}
else
{
swingRestitution = axis2.LimitsRestitution;
swingSpring = axis2.LimitsSpring;
swingDamping = axis2.LimitsDamping;
}
PhysXNativeD6Joint.SetSwingLimit(nativeJoint, yAngle, zAngle, PhysXGeneral.jointLimitContactDistance,
swingRestitution, swingSpring, swingDamping);
}
}
void UpdatePhysXJoint()
{
bool needCreate = PushedToWorld && !Broken;
bool created = nativeJoint != IntPtr.Zero;
if (needCreate == created)
{
return;
}
if (needCreate)
{
if (Math.Abs(Vec3.Dot(axis1.Direction, axis2.Direction)) > .095f)
{
Log.Warning("UniversalJoint: Invalid axes.");
return;
}
//if( Axis1.LimitsEnabled && Axis1.LimitLow > Axis1.LimitHigh )
//{
// Log.Warning( "UniversalJoint: Invalid axis1 limits (low > high)." );
// return;
//}
//if( Axis2.LimitsEnabled && Axis2.LimitLow > Axis2.LimitHigh )
//{
// Log.Warning( "UniversalJoint: Invalid axis2 limits (low > high)." );
// return;
//}
PhysXBody physXBody0 = (PhysXBody)Body1;
PhysXBody physXBody1 = (PhysXBody)Body2;
if ((!physXBody0.Static || !physXBody1.Static) &&
(physXBody0.nativeBody != IntPtr.Zero && physXBody1.nativeBody != IntPtr.Zero))
{
axis1LocalAxis = Body1.Rotation.GetInverse() * axis1.Direction;
axis2LocalAxis = Body1.Rotation.GetInverse() * axis2.Direction;
Mat3 axisMatrix = new Mat3(axis1.Direction, -Vec3.Cross(axis1.Direction, axis2.Direction), axis2.Direction) *
Mat3.FromRotateByZ(MathFunctions.PI / 2);
Quat globalAxisRotation = axisMatrix.ToQuat().GetNormalize();
//Quat globalAxisRotation = new Mat3( axis1.Direction, -Vec3.Cross( axis1.Direction, axis2.Direction ),
// axis2.Direction ).ToQuat().GetNormalize();
Vec3 localPosition0 = physXBody0.Rotation.GetInverse() * (Anchor - physXBody0.Position);
Quat localRotation0 = (physXBody0.Rotation.GetInverse() * globalAxisRotation).GetNormalize();
Vec3 localPosition1 = physXBody1.Rotation.GetInverse() * (Anchor - physXBody1.Position);
Quat localRotation1 = (physXBody1.Rotation.GetInverse() * globalAxisRotation).GetNormalize();
nativeJoint = PhysXNativeWrapper.PhysXNativeD6Joint.Create(
physXBody0.nativeBody, ref localPosition0, ref localRotation0,
physXBody1.nativeBody, ref localPosition1, ref localRotation1);
PhysXNativeD6Joint.SetMotion(nativeJoint, PhysXD6Axis.X, PhysXD6Motion.Locked);
PhysXNativeD6Joint.SetMotion(nativeJoint, PhysXD6Axis.Y, PhysXD6Motion.Locked);
PhysXNativeD6Joint.SetMotion(nativeJoint, PhysXD6Axis.Z, PhysXD6Motion.Locked);
UpdateLimits();
if (ContactsEnabled)
{
PhysXNativeWrapper.PhysXJoint.SetCollisionEnable(nativeJoint, true);
}
UpdatePhysXBreakData();
if (Scene._EnableDebugVisualization)
{
SetVisualizationEnable(true);
}
//PhysXNativeD6Joint.SetProjectionTolerances( nativeJoint, .2f, MathFunctions.PI );
//PhysXNativeWrapper.PhysXJoint.SetProjectionEnable( nativeJoint, true );
}
}
else
{
DestroyPhysXJoint();
}
}