unsafe internal static void CalculateJointStress(dJointID jointID,
out float force, out float torque)
{
IntPtr jointFeedback = Ode.dJointGetFeedbackAsIntPtr(jointID);
if (jointFeedback == IntPtr.Zero)
{
//create jointFeedback and begin use on next simulation step
jointFeedback = Ode.dAlloc((uint)Marshal.SizeOf(typeof(Ode.dJointFeedback)));
//zero memory
unsafe
{
Ode.dJointFeedback *p = (Ode.dJointFeedback *)jointFeedback;
p->f1 = new Ode.dVector3();
p->t1 = new Ode.dVector3();
p->f2 = new Ode.dVector3();
p->t2 = new Ode.dVector3();
}
Ode.dJointSetFeedbackAsIntPtr(jointID, jointFeedback);
force = 0;
torque = 0;
return;
}
Ode.dJointFeedback *ptr = (Ode.dJointFeedback *)jointFeedback;
Vec3 f1 = Convert.ToNet(ptr->f1);
Vec3 t1 = Convert.ToNet(ptr->t1);
Vec3 f2 = Convert.ToNet(ptr->f2);
Vec3 t2 = Convert.ToNet(ptr->t2);
// This is a simplification, but it should still work.
force = (f1 - f2).Length();
torque = (t1 - t2).Length();
}
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();
}
}