internal Geom(dGeomID geom) { id = geom; handle = GCHandle.Alloc(this); NativeMethods.dGeomSetData(id, GCHandle.ToIntPtr(handle)); id.Owner = Space.FromIntPtr(NativeMethods.dGeomGetSpace(id)); }
void nearCallback(IntPtr data, dGeomID o1, dGeomID o2) { dBodyID b1 = ode.dGeomGetBody(o1); dBodyID b2 = ode.dGeomGetBody(o2); const int MAX_CONTACTS = 5; ode.dContact[] contact = new ode.dContact[MAX_CONTACTS]; for (int i = 0; i < MAX_CONTACTS; i++) { contact[i].surface.mode = (int)(ode.dContactType.Bounce | ode.dContactType.SoftCFM); contact[i].surface.mu = Mathf.Infinity; contact[i].surface.mu2 = 0; contact[i].surface.bounce = 0.01f; contact[i].surface.bounce_vel = 0.1f; contact[i].surface.soft_cfm = 0.01f; } int numc = ode.dCollide(o1, o2, MAX_CONTACTS, ref contact[0].geom, System.Runtime.InteropServices.Marshal.SizeOf(typeof(ode.dContact))); if (numc > 0) { for (int i = 0; i < numc; i++) { dJointID c = ode.dJointCreateContact(world, contactgroup, ref contact[i]); ode.dJointAttach(c, b1, b2); } } }
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; }
Body[] DoVolumeCastGeneral(dGeomID volumeCastGeomID, int contactGroup) { int count; IntPtr data; Ode.DoVolumeCast(neoAxisAdditionsID, volumeCastGeomID, contactGroup, out count, out data); if (count == 0) { return(emptyVolumeCastResult); } unsafe { int *pointer = (int *)data; for (int n = 0; n < count; n++) { int shapeDictionaryIndex = *pointer; ODEBody.GeomData geomData = shapesDictionary[shapeDictionaryIndex]; volumeCastResult.AddWithCheckAlreadyContained(geomData.shape.Body); pointer++; } } Body[] result = new Body[volumeCastResult.Count]; volumeCastResult.CopyTo(result, 0); volumeCastResult.Clear(); return(result); }
private Geom(Space space, dGeomID ptr) { this.space = space; this.ptr = ptr; this.id = InstanceMap.Add(this); ode.dGeomSetData(ptr, (IntPtr)id); }
protected override void OnShapeSetMaterialProperty(Shape shape) { GeomData geomData = GetGeomDataByShape(shape); if (geomData != null) { dGeomID geomID = (geomData.transformID != dGeomID.Zero) ? geomData.transformID : geomData.geomID; Ode.SetShapeMaterialProperties(geomID, shape.Hardness, shape.Restitution, shape.DynamicFriction, shape.StaticFriction); } }
protected override Body[] OnVolumeCast(Sphere sphere, int contactGroup) { dGeomID volumeCastGeomID = Ode.dCreateSphere(rootSpaceID, sphere.Radius); Ode.dGeomSetPosition(volumeCastGeomID, sphere.Origin.X, sphere.Origin.Y, sphere.Origin.Z); Body[] result = DoVolumeCastGeneral(volumeCastGeomID, contactGroup); Ode.dGeomDestroy(volumeCastGeomID); return(result); }
protected override void OnShapeSetContactGroup(Shape shape) { base.OnShapeSetContactGroup(shape); GeomData geomData = GetGeomDataByShape(shape); if (geomData != null) { dGeomID geomID = (geomData.transformID != dGeomID.Zero) ? geomData.transformID : geomData.geomID; Ode.SetShapeContractGroup(geomID, shape.ContactGroup); } }
protected override Body[] OnVolumeCast(Box box, int contactGroup) { dGeomID volumeCastGeomID = Ode.dCreateBox(rootSpaceID, box.Extents.X * 2, box.Extents.Y * 2, box.Extents.Z * 2); Mat3 mat3 = box.Axis; Ode.dMatrix3 odeMat3; Convert.ToODE(ref mat3, out odeMat3); Ode.dGeomSetRotation(volumeCastGeomID, ref odeMat3); Ode.dGeomSetPosition(volumeCastGeomID, box.Center.X, box.Center.Y, box.Center.Z); Body[] result = DoVolumeCastGeneral(volumeCastGeomID, contactGroup); Ode.dGeomDestroy(volumeCastGeomID); return(result); }
protected override Body[] OnVolumeCast(Bounds bounds, int contactGroup) { Vec3 size; bounds.GetSize(out size); Vec3 center; bounds.GetCenter(out center); dGeomID volumeCastGeomID = Ode.dCreateBox(rootSpaceID, size.X, size.Y, size.Z); Ode.dGeomSetPosition(volumeCastGeomID, center.X, center.Y, center.Z); Body[] result = DoVolumeCastGeneral(volumeCastGeomID, contactGroup); Ode.dGeomDestroy(volumeCastGeomID); return(result); }
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); } }
void UpdateStaticBodyGeomsTransform() { if (geomDatas != null) { foreach (GeomData geomData in geomDatas) { if (geomData == null) { continue; } Ode.dQuaternion odeQuat; Convert.ToODE(Rotation, out odeQuat); dGeomID geom = (geomData.transformID != dGeomID.Zero) ? geomData.transformID : geomData.geomID; Ode.dGeomSetQuaternion(geom, ref odeQuat); Ode.dGeomSetPosition(geom, Position.X, Position.Y, Position.Z); } } }
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; } }
void DestroyGeomDatas() { if (geomDatas != null) { foreach (GeomData geomData in geomDatas) { if (geomData == null) { continue; } dGeomID geomID = geomData.transformID != dGeomID.Zero ? geomData.transformID : geomData.geomID; Ode.DestroyShapeData(geomID); scene.shapesDictionary.Remove(geomData.shapeDictionaryIndex); if (geomData.transformID != dGeomID.Zero) { Ode.dGeomDestroy(geomData.transformID); } if (geomData.meshGeometryODEData != null) { geomData.meshGeometryODEData.checkRefCounter--; if (geomData.meshGeometryODEData.checkRefCounter < 0) { Log.Fatal("ODEBody: DestroyGeomDatas: Mesh geometry counter < 0."); } } Ode.dGeomDestroy(geomData.geomID); } } geomDatas = null; }
protected override Body[] OnVolumeCast(Capsule capsule, int contactGroup) { Vec3 center; capsule.GetCenter(out center); float length = capsule.GetLength(); Vec3 direction; capsule.GetDirection(out direction); dGeomID volumeCastGeomID = Ode.dCreateCapsule(rootSpaceID, capsule.Radius, length); Quat rotation = Quat.FromDirectionZAxisUp(direction); Mat3 rot; rotation.ToMat3(out rot); Mat3 rotationMat; Mat3.FromRotateByY(MathFunctions.PI / 2, out rotationMat); Mat3 mat3; Mat3.Multiply(ref rot, ref rotationMat, out mat3); Ode.dMatrix3 odeMat3; Convert.ToODE(ref mat3, out odeMat3); Ode.dGeomSetRotation(volumeCastGeomID, ref odeMat3); Ode.dGeomSetPosition(volumeCastGeomID, center.X, center.Y, center.Z); Body[] result = DoVolumeCastGeneral(volumeCastGeomID, contactGroup); Ode.dGeomDestroy(volumeCastGeomID); return(result); }
public extern static void dSpaceAdd( dSpaceID space, dGeomID geom );
public extern static int dGeomTransformGetInfo( dGeomID g );
public extern static void dInfiniteAABB( dGeomID geom, ref Aabb aabb );
public extern static void SetShapeMaterialProperties( dGeomID geomID, float hardness, float bounciness, float dynamicFriction, float staticFriction );
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 SetGeomTriMeshSetRayCallback( dGeomID geomID );
public extern static void DestroyShapeData( dGeomID geomID );
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 void dGeomTriMeshGetTriangle( dGeomID g, int Index, ref dVector3 v0, ref dVector3 v1, ref dVector3 v2 );
public extern static void dSpaceRemove( dSpaceID space, dGeomID geom );
public extern static dTriRayCallback dGeomTriMeshGetRayCallback( dGeomID g );
public extern static void dGeomTriMeshSetArrayCallback( dGeomID g, dTriArrayCallback ArrayCallback );
public extern static int dSpaceQuery( dSpaceID space, dGeomID geom );
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 dGeomTriMeshSetRayCallback( dGeomID g, dTriRayCallback Callback );
public extern static void dGeomTriMeshEnableTC( dGeomID g, int geomClass, int enable );
public extern static void dGeomTriMeshSetData( dGeomID g, dTriMeshDataID Data );
public extern static void dGeomTriMeshClearTCCache( dGeomID g );
public extern static int dGeomTriMeshIsTCEnabled( dGeomID g, int geomClass );
public extern static void dGeomRaySetClosestHit( dGeomID g, int closestHit );
public extern static dTriMeshDataID dGeomTriMeshGetTriMeshDataID( dGeomID g );
public extern static int dGeomRayGetClosestHit( dGeomID g );
public extern static void dGeomTriMeshGetPoint( dGeomID g, int Index, dReal u, dReal v, ref dVector3 Out );
public extern static void dGeomTransformSetGeom( dGeomID g, dGeomID obj );
public extern static void DoVolumeCast( dNeoAxisAdditionsID additions, dGeomID volumeCastGeomID, int contactGroup, out int count, out IntPtr data );
public extern static dGeomID dGeomTransformGetGeom( dGeomID g );
public extern static void CreateShapeData( dGeomID geomID, IntPtr bodyData, int shapeDictionaryIndex, [MarshalAs( UnmanagedType.U1 )] bool shapeTypeMesh, int contactGroup, float hardness, float bounciness, float dynamicFriction, float staticFriction );
public extern static void dGeomTransformSetCleanup( dGeomID g, int mode );
public extern static void SetShapeContractGroup( dGeomID geomID, int contactGroup );
public extern static int dGeomTransformGetCleanup( dGeomID g );
public extern static void SetShapePairDisableContacts( dGeomID geomID1, dGeomID geomID2, [MarshalAs( UnmanagedType.U1 )] bool value );
public extern static void dGeomTransformSetInfo( dGeomID g, int mode );