예제 #1
0
        }// end Step

        private void MoveLinear(float pTimestep, OdeScene _pParentScene)
        {
            if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f))  // requested m_linearMotorDirection is significant
            {
                if (!SafeNativeMethods.BodyIsEnabled(Body))
                {
                    SafeNativeMethods.BodyEnable(Body);
                }

                // add drive to body
                Vector3 addAmount = m_linearMotorDirection / (m_linearMotorTimescale / pTimestep);
                m_lastLinearVelocityVector += (addAmount * 10);  // lastLinearVelocityVector is the current body velocity vector?

                // This will work temporarily, but we really need to compare speed on an axis
                // KF: Limit body velocity to applied velocity?
                if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X))
                {
                    m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X;
                }
                if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y))
                {
                    m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y;
                }
                if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z))
                {
                    m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z;
                }

                // decay applied velocity
                Vector3 decayfraction = ((Vector3.One / (m_linearMotorDecayTimescale / pTimestep)));
                //Console.WriteLine("decay: " + decayfraction);
                m_linearMotorDirection -= m_linearMotorDirection * decayfraction * 0.5f;
                //Console.WriteLine("actual: " + m_linearMotorDirection);
            }
            else
            {        // requested is not significant
                     // if what remains of applied is small, zero it.
                if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f))
                {
                    m_lastLinearVelocityVector = Vector3.Zero;
                }
            }

            // convert requested object velocity to world-referenced vector
            m_dir = m_lastLinearVelocityVector;
            SafeNativeMethods.Quaternion rot = SafeNativeMethods.BodyGetQuaternion(Body);
            Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object

            m_dir *= rotq;                                                // apply obj rotation to velocity vector

            // add Gravity andBuoyancy
            // KF: So far I have found no good method to combine a script-requested
            // .Z velocity and gravity. Therefore only 0g will used script-requested
            // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only.
            Vector3 grav = Vector3.Zero;

            // There is some gravity, make a gravity force vector
            // that is applied after object velocity.
            SafeNativeMethods.Mass objMass;
            SafeNativeMethods.BodyGetMass(Body, out objMass);
            // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
            grav.Z = _pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy);
            // Preserve the current Z velocity
            SafeNativeMethods.Vector3 vel_now = SafeNativeMethods.BodyGetLinearVel(Body);
            m_dir.Z = vel_now.Z;        // Preserve the accumulated falling velocity

            SafeNativeMethods.Vector3 pos = SafeNativeMethods.BodyGetPosition(Body);
//            Vector3 accel = new Vector3(-(m_dir.X - m_lastLinearVelocityVector.X / 0.1f), -(m_dir.Y - m_lastLinearVelocityVector.Y / 0.1f), m_dir.Z - m_lastLinearVelocityVector.Z / 0.1f);
            Vector3 posChange = new Vector3();

            posChange.X = pos.X - m_lastPositionVector.X;
            posChange.Y = pos.Y - m_lastPositionVector.Y;
            posChange.Z = pos.Z - m_lastPositionVector.Z;
            double Zchange = Math.Abs(posChange.Z);

            if (m_BlockingEndPoint != Vector3.Zero)
            {
                if (pos.X >= (m_BlockingEndPoint.X - (float)1))
                {
                    pos.X -= posChange.X + 1;
                    SafeNativeMethods.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
                }
                if (pos.Y >= (m_BlockingEndPoint.Y - (float)1))
                {
                    pos.Y -= posChange.Y + 1;
                    SafeNativeMethods.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
                }
                if (pos.Z >= (m_BlockingEndPoint.Z - (float)1))
                {
                    pos.Z -= posChange.Z + 1;
                    SafeNativeMethods.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
                }
                if (pos.X <= 0)
                {
                    pos.X += posChange.X + 1;
                    SafeNativeMethods.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
                }
                if (pos.Y <= 0)
                {
                    pos.Y += posChange.Y + 1;
                    SafeNativeMethods.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
                }
            }
            if (pos.Z < _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y))
            {
                pos.Z = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + 2;
                SafeNativeMethods.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
            }

            // Check if hovering
            if ((m_Hoverflags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
            {
                // We should hover, get the target height
                if ((m_Hoverflags & VehicleFlag.HOVER_WATER_ONLY) != 0)
                {
                    m_VhoverTargetHeight = _pParentScene.GetWaterLevel() + m_VhoverHeight;
                }
                if ((m_Hoverflags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
                {
                    m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
                }
                if ((m_Hoverflags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0)
                {
                    m_VhoverTargetHeight = m_VhoverHeight;
                }

                if ((m_Hoverflags & VehicleFlag.HOVER_UP_ONLY) != 0)
                {
                    // If body is aready heigher, use its height as target height
                    if (pos.Z > m_VhoverTargetHeight)
                    {
                        m_VhoverTargetHeight = pos.Z;
                    }
                }
                if ((m_Hoverflags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0)
                {
                    if ((pos.Z - m_VhoverTargetHeight) > .2 || (pos.Z - m_VhoverTargetHeight) < -.2)
                    {
                        SafeNativeMethods.BodySetPosition(Body, pos.X, pos.Y, m_VhoverTargetHeight);
                    }
                }
                else
                {
                    float herr0 = pos.Z - m_VhoverTargetHeight;
                    // Replace Vertical speed with correction figure if significant
                    if (Math.Abs(herr0) > 0.01f)
                    {
                        m_dir.Z = -((herr0 * pTimestep * 50.0f) / m_VhoverTimescale);
                        //KF: m_VhoverEfficiency is not yet implemented
                    }
                    else
                    {
                        m_dir.Z = 0f;
                    }
                }

//                m_VhoverEfficiency = 0f;    // 0=boucy, 1=Crit.damped
//                m_VhoverTimescale = 0f;        // time to acheive height
//                pTimestep  is time since last frame,in secs
            }

            if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0)
            {
                //Start Experimental Values
                if (Zchange > .3)
                {
                    grav.Z = (float)(grav.Z * 3);
                }
                if (Zchange > .15)
                {
                    grav.Z = (float)(grav.Z * 2);
                }
                if (Zchange > .75)
                {
                    grav.Z = (float)(grav.Z * 1.5);
                }
                if (Zchange > .05)
                {
                    grav.Z = (float)(grav.Z * 1.25);
                }
                if (Zchange > .025)
                {
                    grav.Z = (float)(grav.Z * 1.125);
                }
                float terraintemp = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
                float postemp     = (pos.Z - terraintemp);
                if (postemp > 2.5f)
                {
                    grav.Z = (float)(grav.Z * 1.037125);
                }
                //End Experimental Values
            }
            if ((m_flags & (VehicleFlag.NO_X)) != 0)
            {
                m_dir.X = 0;
            }
            if ((m_flags & (VehicleFlag.NO_Y)) != 0)
            {
                m_dir.Y = 0;
            }
            if ((m_flags & (VehicleFlag.NO_Z)) != 0)
            {
                m_dir.Z = 0;
            }

            m_lastPositionVector = SafeNativeMethods.BodyGetPosition(Body);

            // Apply velocity
            SafeNativeMethods.BodySetLinearVel(Body, m_dir.X, m_dir.Y, m_dir.Z);
            // apply gravity force
            SafeNativeMethods.BodyAddForce(Body, grav.X, grav.Y, grav.Z);


            // apply friction
            Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep);

            m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount;
        } // end MoveLinear()