void UpdateODEJoint()
{
bool needCreate = PushedToWorld && !Broken;
bool created = jointID != dJointID.Zero;
if (needCreate == created)
{
return;
}
if (needCreate)
{
ODEBody odeBody1 = (ODEBody)Body1;
ODEBody odeBody2 = (ODEBody)Body2;
jointID = Ode.dJointCreateFixed(((ODEPhysicsScene)Scene).worldID, IntPtr.Zero);
Ode.SetJointContactsEnabled(jointID, false); //ContactsEnabled );
Ode.dJointAttach(jointID, odeBody1.bodyID, odeBody2.bodyID);
Ode.dJointSetFixed(jointID);
Ode.BodyDataAddJoint(odeBody1.bodyData, jointID);
Ode.BodyDataAddJoint(odeBody2.bodyData, jointID);
}
else
{
DestroyODEJoint();
}
}
void UpdateODEJoint()
{
bool needCreate = PushedToWorld && !Broken;
bool created = jointID != dJointID.Zero;
if (needCreate == created)
{
return;
}
if (needCreate)
{
ODEBody odeBody1 = (ODEBody)Body1;
ODEBody odeBody2 = (ODEBody)Body2;
jointID = Ode.dJointCreateSlider(((ODEPhysicsScene)Scene).worldID, IntPtr.Zero);
Ode.SetJointContactsEnabled(jointID, ContactsEnabled);
Ode.dJointSetSliderParam(jointID, Ode.dJointParams.dParamFudgeFactor,
Defines.jointFudgeFactor);
Ode.dJointAttach(jointID, odeBody1.bodyID, odeBody2.bodyID);
axis.UpdateToLibrary(true);
Ode.BodyDataAddJoint(odeBody1.bodyData, jointID);
Ode.BodyDataAddJoint(odeBody2.bodyData, jointID);
}
else
{
DestroyODEJoint();
}
}
void UpdateODEJoint()
{
bool needCreate = PushedToWorld && !Broken;
bool created = jointID != dJointID.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;
}
ODEBody odeBody1 = (ODEBody)Body1;
ODEBody odeBody2 = (ODEBody)Body2;
jointID = Ode.dJointCreateUniversal(((ODEPhysicsScene)Scene).worldID, IntPtr.Zero);
Ode.SetJointContactsEnabled(jointID, ContactsEnabled);
Ode.dJointSetUniversalParam(jointID, Ode.dJointParams.dParamFudgeFactor,
Defines.jointFudgeFactor);
Ode.dJointSetUniversalParam(jointID, Ode.dJointParams.dParamFudgeFactor2,
Defines.jointFudgeFactor);
Ode.dJointAttach(jointID, odeBody1.bodyID, odeBody2.bodyID);
Ode.dJointSetUniversalAnchor(jointID, Anchor.X, Anchor.Y, Anchor.Z);
axis1.UpdateToLibrary(true);
axis2.UpdateToLibrary(true);
axis1LocalAxis = Body1.Rotation.GetInverse() * axis1.Direction;
axis2LocalAxis = Body1.Rotation.GetInverse() * axis2.Direction;
Ode.BodyDataAddJoint(odeBody1.bodyData, jointID);
Ode.BodyDataAddJoint(odeBody2.bodyData, jointID);
}
else
{
DestroyODEJoint();
}
}
internal void DestroyODEJoint()
{
if (jointID != dJointID.Zero)
{
ODEBody odeBody1 = (ODEBody)Body1;
ODEBody odeBody2 = (ODEBody)Body2;
Ode.BodyDataRemoveJoint(odeBody1.bodyData, jointID);
Ode.BodyDataRemoveJoint(odeBody2.bodyData, jointID);
Utils.FreeJointFeedback(jointID);
Ode.dJointDestroy(jointID);
jointID = dJointID.Zero;
}
}
void UpdateODEJoint()
{
bool needCreate = PushedToWorld && !Broken;
bool created = jointID != dJointID.Zero;
if (needCreate == created)
{
return;
}
if (needCreate)
{
if (Axis.LimitsEnabled && Axis.LimitLow > Axis.LimitHigh)
{
Log.Warning("HingeJoint: Invalid axis limits (low > high).");
return;
}
ODEBody odeBody1 = (ODEBody)Body1;
ODEBody odeBody2 = (ODEBody)Body2;
jointID = Ode.dJointCreateHinge(((ODEPhysicsScene)Scene).worldID, IntPtr.Zero);
Ode.SetJointContactsEnabled(jointID, ContactsEnabled);
Ode.dJointSetHingeParam(jointID, Ode.dJointParams.dParamFudgeFactor,
Defines.jointFudgeFactor);
Ode.dJointAttach(jointID, odeBody1.bodyID, odeBody2.bodyID);
Ode.dJointSetHingeAnchor(jointID, Anchor.X, Anchor.Y, Anchor.Z);
axis.UpdateToLibrary(true);
Ode.BodyDataAddJoint(odeBody1.bodyData, jointID);
Ode.BodyDataAddJoint(odeBody2.bodyData, jointID);
}
else
{
DestroyODEJoint();
}
}
public override void SetShapePairFlags(Shape shape1, Shape shape2, ShapePairFlags flags)
{
base.SetShapePairFlags(shape1, shape2, flags);
ODEBody body1 = (ODEBody)shape1.Body;
ODEBody body2 = (ODEBody)shape2.Body;
ODEBody.GeomData geomData1 = body1.GetGeomDataByShape(shape1);
ODEBody.GeomData geomData2 = body2.GetGeomDataByShape(shape2);
if (geomData1 != null && geomData2 != null)
{
dGeomID geomID1 = (geomData1.transformID != dGeomID.Zero) ?
geomData1.transformID : geomData1.geomID;
dGeomID geomID2 = (geomData2.transformID != dGeomID.Zero) ?
geomData2.transformID : geomData2.geomID;
bool value = (flags & ShapePairFlags.DisableContacts) != 0;
Ode.SetShapePairDisableContacts(geomID1, geomID2, value);
}
}
public bool CCDCast(ODEBody ccdBody, Vec3 start, Vec3 end, int contactGroup,
out float minDistance)
{
Vec3 direction = end - start;
if (Math.Abs(direction.X) < .0001f && Math.Abs(direction.Y) < .0001f &&
Math.Abs(direction.Z) < .0001f)
{
minDistance = 0;
return(false);
}
Ray ray = new Ray(start, direction);
Vec3 dirNormal = ray.Direction;
float length = dirNormal.Normalize();
Ode.dGeomRaySet(rayCastGeomID, ray.Origin.X, ray.Origin.Y, ray.Origin.Z,
dirNormal.X, dirNormal.Y, dirNormal.Z);
Ode.dGeomRaySetLength(rayCastGeomID, length);
return(Ode.DoCCDCast(neoAxisAdditionsID, ccdBody.bodyID, contactGroup, out minDistance));
}
protected override void OnSimulationStep()
{
foreach (Body body in Bodies)
{
if (!body.Static && !body.Sleeping)
{
ODEBody odeBody = (ODEBody)body;
// Apply linear and angular damping; if using the "add opposing
// forces" method, be sure to do this before calling ODE step
// function.
odeBody.DoDamping();
if (body.CCD)
{
odeBody.ccdLastPosition = odeBody.Position;
}
}
}
int collisionEventCount;
IntPtr collisionEvents;
Ode.DoSimulationStep(neoAxisAdditionsID, out collisionEventCount, out collisionEvents);
//process collision events
{
unsafe
{
Ode.CollisionEventData *pointer = (Ode.CollisionEventData *)collisionEvents;
for (int n = 0; n < collisionEventCount; n++)
{
ODEBody.GeomData geomData1 = shapesDictionary[pointer->shapeDictionaryIndex1];
ODEBody.GeomData geomData2 = shapesDictionary[pointer->shapeDictionaryIndex2];
//// Invalidate the "freely-spinning" parameters.
//geomData1.odeBody.freelySpinning = false;
//geomData2.odeBody.freelySpinning = false;
if (EnableCollisionEvents)
{
Shape shape1 = geomData1.shape;
Shape shape2 = geomData2.shape;
if (IsBodyCollisionEventHandled(shape1.Body) ||
IsBodyCollisionEventHandled(shape2.Body))
{
Vec3 pos;
Vec3 normal;
Convert.ToNet(ref pointer->position, out pos);
Convert.ToNet(ref pointer->normal, out normal);
normal = -normal;
AddCollisionEvent(shape1, shape2, ref pos, ref normal, pointer->depth);
}
}
pointer++;
}
}
}
// Take a simulation step.
Ode.dWorldQuickStep(worldID, StepSize);
// Remove all joints from the contact group.
Ode.dJointGroupEmpty(contactJointGroupID);
//update from ODE
foreach (Body body in Bodies)
{
if (!body.Static)
{
ODEBody odeBody = (ODEBody)body;
odeBody.UpdateDataFromLibrary();
if (!odeBody.Sleeping)
{
//ODE bug fix
//need still?
if (float.IsNaN(odeBody.Position.X))
{
odeBody.Position = odeBody.OldPosition;
}
if (float.IsNaN(odeBody.Rotation.X))
{
odeBody.Rotation = odeBody.OldRotation;
}
//// Fix angular velocities for freely-spinning bodies that have
//// gained angular velocity through explicit integrator inaccuracy.
//odeBody.DoAngularVelocityFix();
if (odeBody.CCD)
{
odeBody.CCDStep();
}
}
}
}
foreach (Joint joint in Joints)
{
switch (joint.JointType)
{
case Joint.Type.Hinge:
((ODEHingeJoint)joint).UpdateDataFromLibrary();
break;
case Joint.Type.Universal:
((ODEUniversalJoint)joint).UpdateDataFromLibrary();
break;
case Joint.Type.Hinge2:
((ODEHinge2Joint)joint).UpdateDataFromLibrary();
break;
case Joint.Type.Ball:
((ODEBallJoint)joint).UpdateDataFromLibrary();
break;
case Joint.Type.Slider:
((ODESliderJoint)joint).UpdateDataFromLibrary();
break;
case Joint.Type.Fixed:
break;
default:
Trace.Assert(false);
break;
}
}
}
public bool CCDCast( ODEBody ccdBody, Vec3 start, Vec3 end, int contactGroup,
out float minDistance )
{
Vec3 direction = end - start;
if( Math.Abs( direction.X ) < .0001f && Math.Abs( direction.Y ) < .0001f &&
Math.Abs( direction.Z ) < .0001f )
{
minDistance = 0;
return false;
}
Ray ray = new Ray( start, direction );
Vec3 dirNormal = ray.Direction;
float length = dirNormal.Normalize();
Ode.dGeomRaySet( rayCastGeomID, ray.Origin.X, ray.Origin.Y, ray.Origin.Z,
dirNormal.X, dirNormal.Y, dirNormal.Z );
Ode.dGeomRaySetLength( rayCastGeomID, length );
return Ode.DoCCDCast( neoAxisAdditionsID, ccdBody.bodyID, contactGroup, out minDistance );
}
void CreateGeomDatas()
{
tempGeomDatasAsList.Clear();
for (int n = 0; n < Shapes.Length; n++)
{
tempGeomDatasAsList.Add(null);
}
dSpaceID bodySpaceID = scene.rootSpaceID;
for (int nShape = 0; nShape < Shapes.Length; nShape++)
{
Shape shape = Shapes[nShape];
GeomData geomData = new GeomData();
geomData.shape = shape;
geomData.odeBody = (ODEBody)shape.Body;
bool identityTransform = shape.Position == Vec3.Zero && shape.Rotation == Quat.Identity;
// No offset transform.
if (identityTransform)
{
geomData.spaceID = bodySpaceID;
}
//create geom
switch (shape.ShapeType)
{
case Shape.Type.Box:
{
BoxShape boxShape = (BoxShape)shape;
geomData.geomID = Ode.dCreateBox(geomData.spaceID, boxShape.Dimensions.X,
boxShape.Dimensions.Y, boxShape.Dimensions.Z);
}
break;
case Shape.Type.Sphere:
{
SphereShape sphereShape = (SphereShape)shape;
geomData.geomID = Ode.dCreateSphere(geomData.spaceID, sphereShape.Radius);
}
break;
case Shape.Type.Capsule:
{
CapsuleShape capsuleShape = (CapsuleShape)shape;
geomData.geomID = Ode.dCreateCapsule(geomData.spaceID, capsuleShape.Radius,
capsuleShape.Length);
}
break;
case Shape.Type.Cylinder:
{
CylinderShape cylinderShape = (CylinderShape)shape;
geomData.geomID = Ode.dCreateCylinder(geomData.spaceID, cylinderShape.Radius,
cylinderShape.Length);
}
break;
case Shape.Type.Mesh:
{
MeshShape meshShape = (MeshShape)shape;
if (!Static)
{
if (!notSupportedMeshesLogInformed)
{
notSupportedMeshesLogInformed = true;
Log.Warning("ODEBody: Dynamic convex and triangle meshes are not " +
"supported by ODE.");
}
Log.Info("ODEBody: Dynamic convex and triangle meshes are not " +
"supported by ODE.");
//ignore shape
continue;
}
//get mesh geometry from cache
PhysicsWorld._MeshGeometry geometry = meshShape._GetMeshGeometry();
//ignore shape
if (geometry == null)
{
Log.Info("ODEBody: Mesh is not initialized. ({0}).", meshShape.MeshName);
continue;
}
ODEPhysicsWorld.MeshGeometryODEData data;
if (geometry.UserData == null)
{
data = new ODEPhysicsWorld.MeshGeometryODEData();
//generate MeshGeometryODEData data
data.triMeshDataID = Ode.dGeomTriMeshDataCreate();
data.verticesCount = geometry.Vertices.Length;
data.indicesCount = geometry.Indices.Length;
data.vertices = (IntPtr)Ode.dAlloc((uint)
(Marshal.SizeOf(typeof(float)) * 3 * data.verticesCount));
data.indices = (IntPtr)Ode.dAlloc((uint)
(Marshal.SizeOf(typeof(int)) * data.indicesCount));
unsafe
{
fixed(Vec3 *source = geometry.Vertices)
{
NativeUtils.CopyMemory(data.vertices, (IntPtr)source,
data.verticesCount * sizeof(Vec3));
}
fixed(int *source = geometry.Indices)
{
NativeUtils.CopyMemory(data.indices, (IntPtr)source,
data.indicesCount * sizeof(int));
}
}
//build ode tri mesh data
Ode.dGeomTriMeshDataBuildSingleAsIntPtr(
data.triMeshDataID,
data.vertices,
Marshal.SizeOf(typeof(float)) * 3,
data.verticesCount,
data.indices,
data.indicesCount,
Marshal.SizeOf(typeof(int)) * 3);
geometry.UserData = data;
}
else
{
data = (ODEPhysicsWorld.MeshGeometryODEData)geometry.UserData;
}
data.checkRefCounter++;
geomData.meshGeometryODEData = data;
geomData.geomID = Ode.dCreateTriMesh(geomData.spaceID,
data.triMeshDataID, null, null, null);
Ode.SetGeomTriMeshSetRayCallback(geomData.geomID);
//unsafe
//{
// float[] planes = new float[]
// {
// 1.0f ,0.0f ,0.0f ,0.25f,
// 0.0f ,1.0f ,0.0f ,0.25f,
// 0.0f ,0.0f ,1.0f ,0.25f,
// -1.0f,0.0f ,0.0f ,0.25f,
// 0.0f ,-1.0f,0.0f ,0.25f,
// 0.0f ,0.0f ,-1.0f,0.25f
// };
// float[] points = new float[]
// {
// 0.25f,0.25f,0.25f,
// -0.25f,0.25f,0.25f,
// 0.25f,-0.25f,0.25f,
// -0.25f,-0.25f,0.25f,
// 0.25f,0.25f,-0.25f,
// -0.25f,0.25f,-0.25f,
// 0.25f,-0.25f,-0.25f,
// -0.25f,-0.25f,-0.25f,
// };
// uint[] polygons = new uint[]
// {
// 4,0,2,6,4,
// 4,1,0,4,5,
// 4,0,1,3,2,
// 4,3,1,5,7,
// 4,2,3,7,6,
// 4,5,4,6,7,
// };
// float* nativePlanes = (float*)Ode.dAlloc( (uint)( sizeof( float ) * planes.Length ) );
// for( int n = 0; n < planes.Length; n++ )
// nativePlanes[ n ] = planes[ n ];
// uint planeCount = 6;
// float* nativePoints = (float*)Ode.dAlloc( (uint)( sizeof( float ) * points.Length ) );
// for( int n = 0; n < points.Length; n++ )
// nativePoints[ n ] = points[ n ];
// uint pointCount = 8;
// uint* nativePolygons = (uint*)Ode.dAlloc( (uint)( sizeof( uint ) * polygons.Length ) );
// for( int n = 0; n < polygons.Length; n++ )
// nativePolygons[ n ] = polygons[ n ];
// //ODEPhysicsWorld.MeshGeometryODEData data;
// //if( geometry.UserData == null )
// //{
// // data = new ODEPhysicsWorld.MeshGeometryODEData();
// //}
// geomData.geomID = Ode.dCreateConvex( geomData.spaceID, nativePlanes,
// planeCount, nativePoints, pointCount, nativePolygons );
//}
}
break;
}
//add geom data to list
tempGeomDatasAsList[nShape] = geomData;
geomData.shape = shape;
geomData.odeBody = (ODEBody)shape.Body;
// Use ODE's geom transform object.
if (!identityTransform)
{
geomData.transformID = Ode.dCreateGeomTransform(bodySpaceID);
}
//set geom to body
if (!Static)
{
if (geomData.transformID == dGeomID.Zero)
{
Ode.dGeomSetBody(geomData.geomID, bodyID);
}
else
{
Ode.dGeomSetBody(geomData.transformID, bodyID);
}
}
if (geomData.transformID != dGeomID.Zero)
{
// Setup geom transform.
Ode.dGeomTransformSetGeom(geomData.transformID, geomData.geomID);
Ode.dQuaternion odeQuat;
Convert.ToODE(shape.Rotation, out odeQuat);
Ode.dGeomSetQuaternion(geomData.geomID, ref odeQuat);
Ode.dGeomSetPosition(geomData.geomID, shape.Position.X,
shape.Position.Y, shape.Position.Z);
}
// Set the GeomData reference for later use (e.g. in collision handling).
geomData.shapeDictionaryIndex = scene.shapesDictionary.Add(geomData);
dGeomID geomID = geomData.transformID != dGeomID.Zero ?
geomData.transformID : geomData.geomID;
Ode.CreateShapeData(geomID, bodyData, geomData.shapeDictionaryIndex,
shape.ShapeType == Shape.Type.Mesh, shape.ContactGroup,
shape.Hardness, shape.Restitution, shape.DynamicFriction, shape.StaticFriction);
//shape pair flags
Dictionary <Shape, ShapePairFlags> list = shape._GetShapePairFlags();
if (list != null)
{
foreach (KeyValuePair <Shape, ShapePairFlags> pair in list)
{
Shape otherShape = pair.Key;
ShapePairFlags flags = pair.Value;
if ((flags & ShapePairFlags.DisableContacts) != 0)
{
ODEBody otherBody = (ODEBody)otherShape.Body;
GeomData otherGeomData = otherBody.GetGeomDataByShape(otherShape);
if (otherGeomData != null)
{
dGeomID otherGeomID = (otherGeomData.transformID != dGeomID.Zero) ?
otherGeomData.transformID : otherGeomData.geomID;
Ode.SetShapePairDisableContacts(geomID, otherGeomID, true);
}
}
}
}
}
geomDatas = tempGeomDatasAsList.ToArray();
tempGeomDatasAsList.Clear();
if (Static)
{
UpdateStaticBodyGeomsTransform();
}
}
void UpdateODEJoint()
{
bool needCreate = PushedToWorld && !Broken;
bool created = jointID != dJointID.Zero;
ODEBody odeBody1 = (ODEBody)Body1;
ODEBody odeBody2 = (ODEBody)Body2;
if (needCreate && (odeBody1.bodyID == dBodyID.Zero || odeBody2.bodyID == dBodyID.Zero))
{
Log.Warning("ODEHinge2Joint: It is necessary that both bodies were not static.");
needCreate = false;
}
if (needCreate == created)
{
return;
}
if (needCreate)
{
if (Math.Abs(Vec3.Dot(axis1.Direction, axis2.Direction)) > .095f)
{
Log.Warning("Hinge2Joint: Invalid axes.");
return;
}
if (Axis1.LimitsEnabled && Axis1.LimitLow > Axis1.LimitHigh)
{
Log.Warning("Hinge2Joint: Invalid axis1 limits (low > high).");
return;
}
if (Axis2.LimitsEnabled && Axis2.LimitLow > Axis2.LimitHigh)
{
Log.Warning("Hinge2Joint: Invalid axis2 limits (low > high).");
return;
}
jointID = Ode.dJointCreateHinge2(((ODEPhysicsScene)Scene).worldID, IntPtr.Zero);
Ode.SetJointContactsEnabled(jointID, ContactsEnabled);
Ode.dJointSetHinge2Param(jointID, Ode.dJointParams.dParamFudgeFactor,
Defines.jointFudgeFactor);
Ode.dJointSetHinge2Param(jointID, Ode.dJointParams.dParamFudgeFactor2,
Defines.jointFudgeFactor);
Ode.dJointAttach(jointID, odeBody1.bodyID, odeBody2.bodyID);
Ode.dJointSetHinge2Anchor(jointID, Anchor.X, Anchor.Y, Anchor.Z);
axis1.UpdateToLibrary(true);
axis2.UpdateToLibrary(true);
UpdateSuspension();
Ode.BodyDataAddJoint(odeBody1.bodyData, jointID);
Ode.BodyDataAddJoint(odeBody2.bodyData, jointID);
}
else
{
DestroyODEJoint();
}
}
