public void Start()
{
// create world
ode.dInitODE2(0);
world = ode.dWorldCreate();
ode.dWorldSetGravity(world, 0, -10, 0);
space = ode.dHashSpaceCreate();
contactgroup = ode.dJointGroupCreate(0);
ground = ode.dCreatePlane(space, 0, 1, 0, 0);
ode.dMass m1 = new ode.dMass();
ball = new OdeObject();
ball.body = ode.dBodyCreate(world); // Crate a rigid body
ode.dMassSetZero(ref m1); // Initialize mass parameters
ode.dMassSetSphereTotal(ref m1, mass, radius); // Calculate a mass parameter
ode.dBodySetMass(ball.body, ref m1);
ode.dBodySetPosition(ball.body, 0, 2, 0);
ode.dBodySetQuaternion(ball.body, Quaternion.AngleAxis(45, Vector3.left));
ode.dBodyGetRotation(ball.body);
ball.geoms.Add(ode.dCreateSphere(space, radius));
ball.target = target;
ode.dGeomSetBody(ball.geoms[0], ball.body);
ode.dBodyAddTorque(ball.body, 5, 0, 0);
int a = 0;
}
public void Create()
{
MaxIterationCount = ODEPhysicsWorld.Instance.defaultMaxIterationCount;
worldID = Ode.dWorldCreate();
//Ode.dVector3 center = new Ode.dVector3( 0, 0, 0 );
//Ode.dVector3 extents = new Ode.dVector3( 1000, 1000, 1000 );
//rootSpaceID = Ode.dQuadTreeSpaceCreate( dSpaceID.Zero, ref center, ref extents, 10 );
rootSpaceID = Ode.dHashSpaceCreate( dSpaceID.Zero );
Ode.dHashSpaceSetLevels( rootSpaceID, ODEPhysicsWorld.Instance.hashSpaceMinLevel,
ODEPhysicsWorld.Instance.hashSpaceMaxLevel );
// Create the ODE contact joint group.
contactJointGroupID = Ode.dJointGroupCreate( 0 );
// Set the ODE global CFM value that will be used by all Joints
// (including contacts). This affects normal Joint constraint
// operation and Joint limits. The user cannot adjust CFM, but
// they can adjust ERP (a.k.a. bounciness/restitution) for materials
// (i.e. contact settings) and Joint limits.
Ode.dWorldSetCFM( worldID, Defines.globalCFM );
// Set the ODE global ERP value. This will only be used for Joints
// under normal conditions, not at their limits. Also, it will not
// affect contacts at all since they use material properties to
// calculate ERP.
Ode.dWorldSetERP( worldID, 0.5f * ( Defines.maxERP + Defines.minERP ) );
Ode.dWorldSetContactSurfaceLayer( worldID, Defines.surfaceLayer );
//MaxIterationCount = maxIterationCount;
//ray for RayCast
rayCastGeomID = Ode.dCreateRay( rootSpaceID, 1 );
Ode.dGeomSetData( rayCastGeomID, IntPtr.Zero );
rayCastResultDistanceComparer = new Comparison<RayCastResult>( SortRayCastResultsMethod );
unsafe
{
Ode.CheckEnumAndStructuresSizes( sizeof( Ode.CollisionEventData ),
sizeof( Ode.RayCastResult ) );
}
neoAxisAdditionsID = Ode.NeoAxisAdditions_Init( Defines.maxContacts, Defines.minERP,
Defines.maxERP, Defines.maxFriction, Defines.bounceThreshold, worldID, rootSpaceID,
rayCastGeomID, contactJointGroupID );
UpdateMaxIterationCount();
UpdateGravity();
UpdateMaxAngularSpeed();
for( int group0 = 0; group0 < 32; group0++ )
for( int group1 = 0; group1 < 32; group1++ )
OnUpdateContactGroups( group0, group1, IsContactGroupsContactable( group0, group1 ) );
}
public OdeSpace()
{
bool useSpatialTrees = true;
// Create the Space without adding it to another Space.
if (!useSpatialTrees)
{
spaceID = dSpaceID.Create();
}
else
{
spaceID = dSpaceID.HashSpaceCreate();
spaceID.SetLevels(3, 9);
}
parentSpaceID = null;
}
public void Destroy()
{
if (shapesDictionary.Count != 0)
{
Log.Warning("ODEPhysicsWorld: OnShutdownLibrary: shapesDictionary.Count != 0.");
}
if (neoAxisAdditionsID != IntPtr.Zero)
{
Ode.NeoAxisAdditions_Shutdown(neoAxisAdditionsID);
neoAxisAdditionsID = IntPtr.Zero;
}
if (rayCastGeomID != dGeomID.Zero)
{
Ode.dGeomDestroy(rayCastGeomID);
rayCastGeomID = dGeomID.Zero;
}
if (rootSpaceID != dSpaceID.Zero)
{
Ode.dSpaceDestroy(rootSpaceID);
rootSpaceID = dSpaceID.Zero;
}
if (worldID != dWorldID.Zero)
{
Ode.dWorldDestroy(worldID);
worldID = dWorldID.Zero;
}
if (contactJointGroupID != dJointGroupID.Zero)
{
Ode.dJointGroupDestroy(contactJointGroupID);
contactJointGroupID = dJointGroupID.Zero;
}
}
public unsafe extern static dGeomID dCreateConvex( dSpaceID space, void*/*dReal*/ planes, uint planecount, void*/*dReal*/ points, uint pointcount, void*/*uint*/ polygons );
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();
}
}
public extern static void dSpaceClean( dSpaceID space );
public extern static dGeomID dSpaceGetGeom( dSpaceID space, int i );
public extern static int dSpaceGetCleanup( dSpaceID space );
public extern static void dSpaceRemove( dSpaceID space, dGeomID geom );
//betauser public extern static dSpaceID dQuadTreeSpaceCreate( dSpaceID space, ref dVector3 Center, ref dVector3 Extents, int Depth );
public void Destroy()
{
if( shapesDictionary.Count != 0 )
Log.Warning( "ODEPhysicsWorld: OnShutdownLibrary: shapesDictionary.Count != 0." );
if( neoAxisAdditionsID != IntPtr.Zero )
{
Ode.NeoAxisAdditions_Shutdown( neoAxisAdditionsID );
neoAxisAdditionsID = IntPtr.Zero;
}
if( rayCastGeomID != dGeomID.Zero )
{
Ode.dGeomDestroy( rayCastGeomID );
rayCastGeomID = dGeomID.Zero;
}
if( rootSpaceID != dSpaceID.Zero )
{
Ode.dSpaceDestroy( rootSpaceID );
rootSpaceID = dSpaceID.Zero;
}
if( worldID != dWorldID.Zero )
{
Ode.dWorldDestroy( worldID );
worldID = dWorldID.Zero;
}
if( contactJointGroupID != dJointGroupID.Zero )
{
Ode.dJointGroupDestroy( contactJointGroupID );
contactJointGroupID = dJointGroupID.Zero;
}
}
public OdeSpace(dSpaceID space)
{
spaceID = space;
parentSpaceID = null;
}
public extern static dSpaceID dSimpleSpaceCreate( dSpaceID space );
public static extern dGeomID dCreateConvex(dSpaceID space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons);
public Space(World world)
{
ptr = ode.dHashSpaceCreate();
this.world = world;
}
public static extern dSpaceID dQuadTreeSpaceCreate(dSpaceID space, ref dReal centerX, ref dReal extentsX, int depth);
public extern static dGeomID dCreateRay( dSpaceID space, dReal length );
public extern static void dHashSpaceGetLevels( dSpaceID space, ref int minlevel, ref int maxlevel );
public extern static dGeomID dCreateGeomTransform( dSpaceID space );
public static extern IntPtr dCreateHeightfield(dSpaceID space, IntPtr data, int bPlaceable);
public extern static dSpaceID dHashSpaceCreate( dSpaceID space );
public void Create()
{
MaxIterationCount = ODEPhysicsWorld.Instance.defaultMaxIterationCount;
worldID = Ode.dWorldCreate();
//Ode.dVector3 center = new Ode.dVector3( 0, 0, 0 );
//Ode.dVector3 extents = new Ode.dVector3( 1000, 1000, 1000 );
//rootSpaceID = Ode.dQuadTreeSpaceCreate( dSpaceID.Zero, ref center, ref extents, 10 );
rootSpaceID = Ode.dHashSpaceCreate(dSpaceID.Zero);
Ode.dHashSpaceSetLevels(rootSpaceID, ODEPhysicsWorld.Instance.hashSpaceMinLevel,
ODEPhysicsWorld.Instance.hashSpaceMaxLevel);
// Create the ODE contact joint group.
contactJointGroupID = Ode.dJointGroupCreate(0);
// Set the ODE global CFM value that will be used by all Joints
// (including contacts). This affects normal Joint constraint
// operation and Joint limits. The user cannot adjust CFM, but
// they can adjust ERP (a.k.a. bounciness/restitution) for materials
// (i.e. contact settings) and Joint limits.
Ode.dWorldSetCFM(worldID, Defines.globalCFM);
// Set the ODE global ERP value. This will only be used for Joints
// under normal conditions, not at their limits. Also, it will not
// affect contacts at all since they use material properties to
// calculate ERP.
Ode.dWorldSetERP(worldID, 0.5f * (Defines.maxERP + Defines.minERP));
Ode.dWorldSetContactSurfaceLayer(worldID, Defines.surfaceLayer);
//MaxIterationCount = maxIterationCount;
//ray for RayCast
rayCastGeomID = Ode.dCreateRay(rootSpaceID, 1);
Ode.dGeomSetData(rayCastGeomID, IntPtr.Zero);
rayCastResultDistanceComparer = new Comparison <RayCastResult>(SortRayCastResultsMethod);
unsafe
{
Ode.CheckEnumAndStructuresSizes(sizeof(Ode.CollisionEventData),
sizeof(Ode.RayCastResult));
}
neoAxisAdditionsID = Ode.NeoAxisAdditions_Init(Defines.maxContacts, Defines.minERP,
Defines.maxERP, Defines.maxFriction, Defines.bounceThreshold, worldID, rootSpaceID,
rayCastGeomID, contactJointGroupID);
UpdateMaxIterationCount();
UpdateGravity();
UpdateMaxAngularSpeed();
for (int group0 = 0; group0 < 32; group0++)
{
for (int group1 = 0; group1 < 32; group1++)
{
OnUpdateContactGroups(group0, group1, IsContactGroupsContactable(group0, group1));
}
}
}
public extern static void dSpaceDestroy( dSpaceID space );
internal Space(dSpaceID space)
: base(space)
{
id = space;
}
public extern static void dSpaceSetCleanup( dSpaceID space, int mode );
public extern static void dSpaceCollide( dSpaceID space, IntPtr data, dNearCallback callback );
public extern static void dSpaceAdd( dSpaceID space, dGeomID geom );
public extern static dGeomID dCreateSphere( dSpaceID space, dReal radius );
public extern static int dSpaceQuery( dSpaceID space, dGeomID geom );
public extern static dGeomID dCreateBox( dSpaceID space, dReal lx, dReal ly, dReal lz );
public extern static int dSpaceGetNumGeoms( dSpaceID space );
public extern static dGeomID dCreatePlane( dSpaceID space, dReal a, dReal b, dReal c, dReal d );
public extern static dGeomID dCreateTriMesh( dSpaceID space, dTriMeshDataID Data, dTriCallback Callback, dTriArrayCallback ArrayCallback, dTriRayCallback RayCallback );
public extern static dGeomID dCreateCapsule( dSpaceID space, dReal radius, dReal length );
public extern static dNeoAxisAdditionsID NeoAxisAdditions_Init( int maxContacts, float minERP, float maxERP, float maxFriction, float bounceThreshold, dWorldID worldID, dSpaceID rootSpaceID, dGeomID rayCastGeomID, dJointGroupID contactJointGroupID );
public extern static dGeomID dCreateCylinder( dSpaceID space, dReal radius, dReal length );
