Пример #1
0
        public override Vector3 GetLocalLinearVelAtLocalPos(Vector3 p)
        {
            if (!data.IsStatic)
            {
                // First find the global velocity at the given point.

                Vector3 vel = bodyID.GetRelPointVel(p);

                // Now convert the velocity from global to local coordinates.
                vel = bodyID.BodyVectorFromWorld(vel);

                return(vel);
            }
            else
            {
                return(Vector3.Zero);
            }
        }
Пример #2
0
        protected override void StepPhysics()
        {
            // Apply linear and angular damping; if using the "add opposing
            // forces" method, be sure to do this before calling ODE step
            // function.
            if (solidList != null)
            {
                foreach (OdeSolid solid in solidList)
                {
                    if (!solid.Static)
                    {
                        if (solid.Sleeping)
                        {
                            // Reset velocities, force, & torques of objects that go
                            // to sleep; ideally, this should happen in the ODE
                            // source only once - when the object initially goes to
                            // sleep.

                            /*ODE.Body body = solid.InternalGetBodyID();
                             * body.ApplyLinearVelocityDrag(0, 0, 0);
                             * body.ApplyAngularVelocityDrag(0, 0, 0);
                             * body.AddForce(0, 0, 0);
                             * body.AddTorque(0, 0, 0);*/
                            IntPtr body = solid.InternalGetBodyID();
                            Tao.Ode.Ode.dBodySetLinearVel(body, 0, 0, 0);
                            Tao.Ode.Ode.dBodySetAngularVel(body, 0, 0, 0);
                            Tao.Ode.Ode.dBodySetForce(body, 0, 0, 0);
                            Tao.Ode.Ode.dBodySetTorque(body, 0, 0, 0);
                        }
                        else
                        {
                            // Dampen Solid motion.  3 possible methods:
                            // 1) apply a force/torque in the opposite direction of
                            // motion scaled by the body's velocity
                            // 2) same as 1, but scale the opposing force by
                            // the body's momentum (this automatically handles
                            // different mass values)
                            // 3) reduce the body's linear and angular velocity by
                            // scaling them by 1 - damping * stepsize

                            /*ODE.Body body = solid.InternalGetBody();
                             * ODE.Mass mass = body.Mass;*/

                            dBodyID           body = solid.InternalGetBodyID();
                            Tao.Ode.Ode.dMass mass = body.Mass;
                            //Tao.Ode.Ode.dBodyGetMass(body, ref mass); // (mike)

                            // Method 2
                            // Since the ODE mass.I inertia matrix is local, angular
                            // velocity and torque also need to be local.

                            // Linear damping
                            float linearDamping = solid.LinearDamping;
                            if (0 != linearDamping)
                            {
                                Vector3 lVelGlobal = Tao.Ode.Ode.dBodyGetLinearVel(body); // C# compiler automatically calls an implicit convertion here

                                // The damping force depends on the damping amount,
                                // mass, and velocity (i.e. damping amount and
                                // momentum).
                                float   dampingFactor = -linearDamping * mass.mass;
                                Vector3 dampingForce  = new Vector3(dampingFactor * lVelGlobal.X, dampingFactor * lVelGlobal.Y, dampingFactor * lVelGlobal.Z);

                                // Add a global force opposite to the global linear
                                // velocity.
                                body.AddForce(dampingForce);
                                //(mike)//Tao.Ode.Ode.dBodyAddForce(body, dampingForce.X, dampingForce.Y, dampingForce.Z);
                            }

                            // Angular damping
                            float angularDamping = solid.AngularDamping;
                            if (0 != angularDamping)
                            {
                                Vector3 aVelGlobal = Tao.Ode.Ode.dBodyGetAngularVel(body); // C# compiler automatically calls an implicit convertion here

                                //(mike) //Tao.Ode.Ode.dBodyVectorFromWorld(body, aVelGlobal.X, aVelGlobal.Y, aVelGlobal.Z, ref temp);

                                Vector3 aVelLocal = body.BodyVectorFromWorld(aVelGlobal);

                                // The damping force depends on the damping amount,
                                // mass, and velocity (i.e. damping amount and
                                // momentum).
                                float   dampingFactor = -angularDamping;
                                Vector3 aMomentum;

                                // Make adjustments for inertia tensor.
                                //dMULTIPLYOP0_331( aMomentum, = , mass.I, aVelLocal ); (KleMiX) ???
                                aMomentum.X = mass.I.M00 * aVelLocal.X + mass.I.M01 * aVelLocal.Y + aVelLocal.X + mass.I.M02 * aVelLocal.Z;
                                aMomentum.Y = mass.I.M10 * aVelLocal.X + mass.I.M11 * aVelLocal.Y + aVelLocal.X + mass.I.M12 * aVelLocal.Z;
                                aMomentum.Z = mass.I.M20 * aVelLocal.X + mass.I.M21 * aVelLocal.Y + aVelLocal.X + mass.I.M22 * aVelLocal.Z;

                                Vector3 dampingTorque = new Vector3(dampingFactor * aMomentum.X, dampingFactor * aMomentum.Y, dampingFactor * aMomentum.Z);

                                // Add a local torque opposite to the local angular
                                // velocity.
                                //body.AddRelativeTorque(dampingTorque);
                                Tao.Ode.Ode.dBodyAddRelTorque(body, dampingTorque.X, dampingTorque.Y, dampingTorque.Z);
                            }
                        }
                    }
                }
            }

            // Do collision detection; add contacts to the contact joint group.
            //space.Collide();
            Tao.Ode.Ode.dSpaceCollide(space, GCHandle.ToIntPtr(GCHandle.Alloc(this)),
                                      OdeHelper.CollisionCallbackRunner);

            // Take a simulation step.
            if (SolverType.QuickStep == solverType)
            {
                //world.QuickStep(stepSize, quickWorldIterations);
                Tao.Ode.Ode.dWorldQuickStep(world, stepSize);
            }
            else
            {
                //world.TimeStep(stepSize);
                Tao.Ode.Ode.dWorldStep(world, stepSize);
            }

            // Remove all joints from the contact group.
            //contactJointGroup.Clear();
            Tao.Ode.Ode.dJointGroupEmpty(contactJointGroup);

            // Fix angular velocities for freely-spinning bodies that have
            // gained angular velocity through explicit integrator inaccuracy.

            if (solidList != null)
            {
                foreach (OdeSolid s in solidList)
                {
                    if (!s.Sleeping && !s.Static)
                    {
                        s.InternalDoAngularVelFix();
                    }
                }
            }
        }