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;
            }
        }
Example #5
0
        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();
            }
        }
Example #6
0
        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);
            }
        }
Example #7
0
        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));
        }
Example #8
0
        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;
                }
            }
        }
Example #9
0
		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 );
		}
Example #10
0
        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();
            }
        }