Exemplo n.º 1
0
 private void ChildSetGeom(UniverseODEPrim odePrim)
 {
     DestroyBody();
     MakeBody();
 }
Exemplo n.º 2
0
 private void SetInStaticSpace(UniverseODEPrim prm)
 {
     if (prm.m_targetSpace != IntPtr.Zero && prm.m_targetSpace == _parent_scene.space)
     {
         if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom))
             d.SpaceRemove(prm.m_targetSpace, prm.prim_geom);
     }
     prm.m_targetSpace = _parent_scene.calculateSpaceForGeom(prm._position);
     d.SpaceAdd(prm.m_targetSpace, prm.prim_geom);
 }
Exemplo n.º 3
0
        private void changelink(UniverseODEPrim newparent)
        {
            // If the newly set parent is not null
            // create link
            if (_parent == null && newparent != null)
            {
                newparent.ParentPrim(this);
            }
                // If the newly set parent is null
                // destroy link
            else if (_parent != null)
            {
                if (_parent is UniverseODEPrim)
                {
                    if (newparent != _parent)
                    {
                        UniverseODEPrim obj = (UniverseODEPrim) _parent;
                        obj.ChildDelink(this);
                        childPrim = false;

                        if (newparent != null)
                        {
                            newparent.ParentPrim(this);
                        }
                    }
                }
            }

            _parent = newparent;
        }
Exemplo n.º 4
0
        private void ChildRemove(UniverseODEPrim odePrim)
        {
            // Okay, we have a delinked child.. destroy all body and remake
            if (odePrim != this && !childrenPrim.Contains(odePrim))
                return;

            DestroyBody();

            if (odePrim == this)
            {
                UniverseODEPrim newroot = null;
                lock (childrenPrim)
                {
                    if (childrenPrim.Count > 0)
                    {
                        newroot = childrenPrim[0];
                        childrenPrim.RemoveAt(0);
                        foreach (UniverseODEPrim prm in childrenPrim)
                        {
                            newroot.childrenPrim.Add(prm);
                        }
                        childrenPrim.Clear();
                    }
                    if (newroot != null)
                    {
                        newroot.childPrim = false;
                        newroot._parent = null;
                        newroot.MakeBody();
                    }
                }
                return;
            }
            else
            {
                lock (childrenPrim)
                {
                    childrenPrim.Remove(odePrim);
                    odePrim.childPrim = false;
                    odePrim._parent = null;
                }
            }

            MakeBody();
        }
        // end Step
        private void MoveLinear(float pTimestep, UniverseODEPhysicsScene _pParentScene, UniverseODEPrim parent)
        {
            bool ishovering = false;
            bool bypass_buoyancy = false;
            d.Vector3 dpos = d.BodyGetPosition(Body);
            d.Vector3 dvel_now = d.BodyGetLinearVel(Body);
            d.Quaternion drotq_now = d.BodyGetQuaternion(Body);

            Vector3 pos = new Vector3(dpos.X, dpos.Y, dpos.Z);
            Vector3 vel_now = new Vector3(dvel_now.X, dvel_now.Y, dvel_now.Z);
            Quaternion rotq = new Quaternion(drotq_now.X, drotq_now.Y, drotq_now.Z, drotq_now.W);
            rotq *= m_referenceFrame; //add reference rotation to rotq
            Quaternion irotq = new Quaternion(-rotq.X, -rotq.Y, -rotq.Z, rotq.W);

            m_newVelocity = Vector3.Zero;

            if (!(m_lastPositionVector.X == 0 &&
                  m_lastPositionVector.Y == 0 &&
                  m_lastPositionVector.Z == 0))
            {
                ///Only do this if we have a last position
                m_lastposChange.X = pos.X - m_lastPositionVector.X;
                m_lastposChange.Y = pos.Y - m_lastPositionVector.Y;
                m_lastposChange.Z = pos.Z - m_lastPositionVector.Z;
            }

            #region Blocking Change

            if (m_BlockingEndPoint != Vector3.Zero)
            {
                bool needUpdateBody = false;
                if(pos.X >= (m_BlockingEndPoint.X - 1)) {
                    pos.X -= m_lastposChange.X + 1;
                    needUpdateBody = true;
                }
                if(pos.Y >= (m_BlockingEndPoint.Y - 1)) {
                    pos.Y -= m_lastposChange.Y + 1;
                    needUpdateBody = true;
                }
                if(pos.Z >= (m_BlockingEndPoint.Z - 1)) {
                    pos.Z -= m_lastposChange.Z + 1;
                    needUpdateBody = true;
                }
                if(pos.X <= 0) {
                    pos.X += m_lastposChange.X + 1;
                    needUpdateBody = true;
                }
                if(pos.Y <= 0) {
                    pos.Y += m_lastposChange.Y + 1;
                    needUpdateBody = true;
                }
                if(needUpdateBody)
                    d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
            }

            #endregion

            #region Terrain checks

            float terrainHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
            if(pos.Z < terrainHeight - 5) {
                pos.Z = terrainHeight + 2;
                m_lastPositionVector = pos;
                d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
            }
            else if(pos.Z < terrainHeight) {
                m_newVelocity.Z += 1;
            }

            #endregion

            #region Hover

            Vector3 hovervel = Vector3.Zero;
            if(m_VhoverTimescale * pTimestep <= 300.0f && m_VhoverHeight > 0.0f) {
                ishovering = true;

                if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0) {
                    m_VhoverTargetHeight = (float) _pParentScene.GetWaterLevel(pos.X, pos.Y) + 0.3f + m_VhoverHeight;
                }
                else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0) {
                    m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
                }
                else if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0) {
                    m_VhoverTargetHeight = m_VhoverHeight;
                }
                else {
                    float waterlevel = (float)_pParentScene.GetWaterLevel(pos.X, pos.Y) + 0.3f;
                    float terrainlevel = (float)_pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
                    if(waterlevel > terrainlevel) {
                        m_VhoverTargetHeight = waterlevel + m_VhoverHeight;
                    }
                    else {
                        m_VhoverTargetHeight = terrainlevel + m_VhoverHeight;
                    }
                }

                float tempHoverHeight = m_VhoverTargetHeight;
                if((m_flags & VehicleFlag.HOVER_UP_ONLY) != 0) {
                    // If body is aready heigher, use its height as target height
                    if (pos.Z > tempHoverHeight) {
                        tempHoverHeight = pos.Z;
                        bypass_buoyancy = true; //emulate sl bug
                    }
                }
                if((m_flags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0) {
                    if((pos.Z - tempHoverHeight) > .2 || (pos.Z - tempHoverHeight) < -.2) {
                        float h = tempHoverHeight;
                        float groundHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
                        if(groundHeight >= tempHoverHeight)
                            h = groundHeight;

                        d.BodySetPosition(Body, pos.X, pos.Y, h);
                    }
                }
                else {
                    hovervel.Z -= ((dvel_now.Z * 0.1f * m_VhoverEfficiency) + (pos.Z - tempHoverHeight)) / m_VhoverTimescale;
                    hovervel.Z *= 7.0f * (1.0f + m_VhoverEfficiency);

                    if(hovervel.Z > 50.0f) hovervel.Z = 50.0f;
                    if(hovervel.Z < -50.0f) hovervel.Z = -50.0f;
                }
            }

            #endregion

            #region limitations

            //limit maximum velocity
            if(vel_now.LengthSquared() > 1e6f) {
                vel_now /= vel_now.Length();
                vel_now *= 1000f;
                d.BodySetLinearVel(Body, vel_now.X, vel_now.Y, vel_now.Z);
            }

            //block movement in x and y when low velocity
            bool enable_ode_gravity = true;
            if(vel_now.LengthSquared() < 0.02f) {
                d.BodySetLinearVel(Body, 0.0f, 0.0f, 0.0f);
                vel_now = Vector3.Zero;
                if(parent.LinkSetIsColliding)
                    enable_ode_gravity = false;
            }

            #endregion

            #region Linear motors

            //cancel directions of linear friction for certain vehicles without having effect on ode gravity
            Vector3 vt_vel_now = vel_now;
            bool no_grav_calc = false;
            if((Type != Vehicle.TYPE_AIRPLANE && Type != Vehicle.TYPE_BALLOON) && m_VehicleBuoyancy != 1.0f) {
                vt_vel_now.Z = 0.0f;
                no_grav_calc = true;
            }

            if(!bypass_buoyancy) {
                //apply additional gravity force over ode gravity
                if(m_VehicleBuoyancy == 1.0f) enable_ode_gravity = false;
                else if(m_VehicleBuoyancy != 0.0f && enable_ode_gravity) {
                    float grav = _pParentScene.gravityz * parent.GravityMultiplier * -m_VehicleBuoyancy;
                    m_newVelocity.Z += grav * Mass;
                }
            }

            //set ode gravity
            d.BodySetGravityMode(Body, enable_ode_gravity);

            //add default linear friction (mimic sl friction as much as possible)
            float initialFriction = 0.055f;
            float defaultFriction = 180f;

            Vector3 friction = Vector3.Zero;
            if(parent.LinkSetIsColliding || ishovering) {
                if(vt_vel_now.X > 0.0f) friction.X += initialFriction;
                if(vt_vel_now.Y > 0.0f) friction.Y += initialFriction;
                if(vt_vel_now.Z > 0.0f) friction.Z += initialFriction;
                if(vt_vel_now.X < 0.0f) friction.X -= initialFriction;
                if(vt_vel_now.Y < 0.0f) friction.Y -= initialFriction;
                if(vt_vel_now.Z < 0.0f) friction.Z -= initialFriction;
                friction += vt_vel_now / defaultFriction;
                friction *= irotq;
            }

            //world -> body orientation
            vel_now *= irotq;
            vt_vel_now *= irotq;

            //add linear friction
            if(vt_vel_now.X > 0.0f)
                friction.X += vt_vel_now.X * vt_vel_now.X / m_linearFrictionTimescale.X;
            else
                friction.X -= vt_vel_now.X * vt_vel_now.X / m_linearFrictionTimescale.X;

            if(vt_vel_now.Y > 0.0f)
                friction.Y += vt_vel_now.Y * vt_vel_now.Y / m_linearFrictionTimescale.Y;
            else
                friction.Y -= vt_vel_now.Y * vt_vel_now.Y / m_linearFrictionTimescale.Y;

            if(vt_vel_now.Z > 0.0f)
                friction.Z += vt_vel_now.Z * vt_vel_now.Z / m_linearFrictionTimescale.Z;
            else
                friction.Z -= vt_vel_now.Z * vt_vel_now.Z / m_linearFrictionTimescale.Z;

            friction /= 1.35f; //1.5f;

            //add linear forces
            //not the best solution, but it is really close to sl motor velocity, and just works
            Vector3 motorVelocity = (m_linearMotorDirection * 3.0f - vel_now) / m_linearMotorTimescale / 5.0f;  //2.8f;
            Vector3 motorfrictionamp = new Vector3(4.0f, 4.0f, 4.0f);
            Vector3 motorfrictionstart = new Vector3(1.0f, 1.0f, 1.0f);
            motorVelocity *= motorfrictionstart + motorfrictionamp / (m_linearFrictionTimescale * pTimestep);
            float addVel = 0.15f;
            if(motorVelocity.LengthSquared() > 0.01f) {
                if(motorVelocity.X > 0.0f) motorVelocity.X += addVel;
                if(motorVelocity.Y > 0.0f) motorVelocity.Y += addVel;
                if(motorVelocity.Z > 0.0f) motorVelocity.Z += addVel;
                if(motorVelocity.X < 0.0f) motorVelocity.X -= addVel;
                if(motorVelocity.Y < 0.0f) motorVelocity.Y -= addVel;
                if(motorVelocity.Z < 0.0f) motorVelocity.Z -= addVel;
            }

            //free run
            if(vel_now.X > m_linearMotorDirection.X && m_linearMotorDirection.X >= 0.0f) motorVelocity.X = 0.0f;
            if(vel_now.Y > m_linearMotorDirection.Y && m_linearMotorDirection.Y >= 0.0f) motorVelocity.Y = 0.0f;
            if(vel_now.Z > m_linearMotorDirection.Z && m_linearMotorDirection.Z >= 0.0f) motorVelocity.Z = 0.0f;

            if(vel_now.X < m_linearMotorDirection.X && m_linearMotorDirection.X <= 0.0f) motorVelocity.X = 0.0f;
            if(vel_now.Y < m_linearMotorDirection.Y && m_linearMotorDirection.Y <= 0.0f) motorVelocity.Y = 0.0f;
            if(vel_now.Z < m_linearMotorDirection.Z && m_linearMotorDirection.Z <= 0.0f) motorVelocity.Z = 0.0f;

            //decay linear motor
            m_linearMotorDirection *= (1.0f - 1.0f/m_linearMotorDecayTimescale);

            #endregion

            #region Deflection

            //does only deflect on x axis from world orientation with z axis rotated to body
            //it is easier to filter out gravity deflection for vehicles(car) without rotation problems
            Quaternion irotq_z = irotq;
            irotq_z.X = 0.0f;
            irotq_z.Y = 0.0f;
            float mag = (float)Math.Sqrt(irotq_z.W * irotq_z.W + irotq_z.Z * irotq_z.Z); //normalize
            irotq_z.W /= mag;
            irotq_z.Z /= mag;

            Vector3 vel_defl = new Vector3(dvel_now.X, dvel_now.Y, dvel_now.Z);
            vel_defl *= irotq_z;
            if(no_grav_calc) {
                vel_defl.Z = 0.0f;
                if(!parent.LinkSetIsColliding) vel_defl.Y = 0.0f;
            }

            Vector3 deflection = vel_defl / m_linearDeflectionTimescale * m_linearDeflectionEfficiency * 100.0f;

            float deflectionLengthY = Math.Abs(deflection.Y);
            float deflectionLengthX = Math.Abs(deflection.X);

            deflection.Z = 0.0f;
            if((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) == 0) {
                deflection.Z = deflectionLengthX;
                deflection.X = -deflection.X;
            }

            if(vel_defl.X < 0.0f) deflection.X = -deflectionLengthY;
            else if(vel_defl.X >= 0.0f) deflection.X = deflectionLengthY;
            deflection.Y = -deflection.Y;

            irotq_z.W = -irotq_z.W;
            deflection *= irotq_z;

            #endregion

            #region Deal with tainted forces

            Vector3 TaintedForce = new Vector3();
            if(m_forcelist.Count != 0) {
                try {
                    TaintedForce = m_forcelist.Aggregate(TaintedForce, (current, t) => current + (t));
                }
                catch(IndexOutOfRangeException) {
                    TaintedForce = Vector3.Zero;
                }
                catch(ArgumentOutOfRangeException) {
                    TaintedForce = Vector3.Zero;
                }
                m_forcelist = new List<Vector3>();
            }

            #endregion

            #region Add Forces

            //add forces
            m_newVelocity -= (friction *= Mass / pTimestep);
            m_newVelocity += TaintedForce;
            motorVelocity *= Mass / pTimestep;

            #endregion

            #region No X,Y,Z

            if((m_flags & (VehicleFlag.NO_X)) != 0)
                m_newVelocity.X = -vel_now.X * Mass / pTimestep;
            if((m_flags & (VehicleFlag.NO_Y)) != 0)
                m_newVelocity.Y = -vel_now.Y * Mass / pTimestep;
            if((m_flags & (VehicleFlag.NO_Z)) != 0)
                m_newVelocity.Z = -vel_now.Z * Mass / pTimestep;

            #endregion

            m_newVelocity *= rotq;
            m_newVelocity += (hovervel *= Mass / pTimestep);

            if(parent.LinkSetIsColliding || Type == Vehicle.TYPE_AIRPLANE || Type == Vehicle.TYPE_BALLOON || ishovering) {
                m_newVelocity += deflection;

                motorVelocity *= rotq;

                if((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0 && motorVelocity.Z > 0.0f) motorVelocity.Z = 0.0f;
                m_newVelocity += motorVelocity;
            }

            d.BodyAddForce(Body, m_newVelocity.X, m_newVelocity.Y, m_newVelocity.Z);
        }
Exemplo n.º 6
0
        // I'm the parent
        // prim is the child
        public void ParentPrim(UniverseODEPrim prim)
        {
            //Console.WriteLine("ParentPrim  " + m_primName);
            if (this.m_localID != prim.m_localID)
            {
                DestroyBody();

                lock (childrenPrim)
                {
                    foreach (UniverseODEPrim prm in prim.childrenPrim.Where(prm => !childrenPrim.Contains(prm)))
                    {
                        childrenPrim.Add(prm);
                    }

                    if (!childrenPrim.Contains(prim)) // must allow full reconstruction
                        childrenPrim.Add(prim);
                }
                //Remove old children
                prim.childrenPrim.Clear();
                prim.childPrim = true;
                prim._parent = this;

                if (prim.Body != IntPtr.Zero)
                {
                    prim.DestroyBody(); // don't loose bodies around
                    prim.Body = IntPtr.Zero;
                }
                MakeBody(); // full nasty reconstruction
            }
        }
        private void MoveAngular(float pTimestep, UniverseODEPhysicsScene _pParentScene, UniverseODEPrim parent)
        {
            bool ishovering = false;
            d.Vector3 d_angularVelocity = d.BodyGetAngularVel(Body);
            d.Vector3 d_lin_vel_now = d.BodyGetLinearVel(Body);
            d.Quaternion drotq = d.BodyGetQuaternion(Body);
            Quaternion rotq = new Quaternion(drotq.X, drotq.Y, drotq.Z, drotq.W);
            rotq *= m_referenceFrame; //add reference rotation to rotq
            Quaternion irotq = new Quaternion(-rotq.X, -rotq.Y, -rotq.Z, rotq.W);
            Vector3 angularVelocity = new Vector3(d_angularVelocity.X, d_angularVelocity.Y, d_angularVelocity.Z);
            Vector3 linearVelocity = new Vector3(d_lin_vel_now.X, d_lin_vel_now.Y, d_lin_vel_now.Z);

            Vector3 friction = Vector3.Zero;
            Vector3 vertattr = Vector3.Zero;
            Vector3 deflection = Vector3.Zero;
            Vector3 banking = Vector3.Zero;

            //limit maximum rotation speed
            if(angularVelocity.LengthSquared() > 1e3f) {
                angularVelocity = Vector3.Zero;
                d.BodySetAngularVel(Body, angularVelocity.X, angularVelocity.Y, angularVelocity.Z);
            }

            angularVelocity *= irotq; //world to body orientation

            if(m_VhoverTimescale * pTimestep <= 300.0f && m_VhoverHeight > 0.0f) ishovering = true;

            #region Angular motor
            Vector3 motorDirection = Vector3.Zero;

            if(Type == Vehicle.TYPE_BOAT) {
                //keep z flat for boats, no sidediving lol
                Vector3 tmp = new Vector3(0.0f, 0.0f, m_angularMotorDirection.Z);
                m_angularMotorDirection.Z = 0.0f;
                m_angularMotorDirection += tmp * irotq;
            }

            if(parent.LinkSetIsColliding || Type == Vehicle.TYPE_AIRPLANE || Type == Vehicle.TYPE_BALLOON || ishovering){
                motorDirection = m_angularMotorDirection * 0.34f; //0.3f;
            }

            m_angularMotorVelocity.X = (motorDirection.X - angularVelocity.X) / m_angularMotorTimescale;
            m_angularMotorVelocity.Y = (motorDirection.Y - angularVelocity.Y) / m_angularMotorTimescale;
            m_angularMotorVelocity.Z = (motorDirection.Z - angularVelocity.Z) / m_angularMotorTimescale;
            m_angularMotorDirection *= (1.0f - 1.0f/m_angularMotorDecayTimescale);

            if(m_angularMotorDirection.LengthSquared() > 0.0f)
            {
                if(angularVelocity.X > m_angularMotorDirection.X && m_angularMotorDirection.X >= 0.0f) m_angularMotorVelocity.X = 0.0f;
                if(angularVelocity.Y > m_angularMotorDirection.Y && m_angularMotorDirection.Y >= 0.0f) m_angularMotorVelocity.Y = 0.0f;
                if(angularVelocity.Z > m_angularMotorDirection.Z && m_angularMotorDirection.Z >= 0.0f) m_angularMotorVelocity.Z = 0.0f;

                if(angularVelocity.X < m_angularMotorDirection.X && m_angularMotorDirection.X <= 0.0f) m_angularMotorVelocity.X = 0.0f;
                if(angularVelocity.Y < m_angularMotorDirection.Y && m_angularMotorDirection.Y <= 0.0f) m_angularMotorVelocity.Y = 0.0f;
                if(angularVelocity.Z < m_angularMotorDirection.Z && m_angularMotorDirection.Z <= 0.0f) m_angularMotorVelocity.Z = 0.0f;
            }

            #endregion

            #region friction

            float initialFriction = 0.0001f;
            if(angularVelocity.X > initialFriction) friction.X += initialFriction;
            if(angularVelocity.Y > initialFriction) friction.Y += initialFriction;
            if(angularVelocity.Z > initialFriction) friction.Z += initialFriction;
            if(angularVelocity.X < -initialFriction) friction.X -= initialFriction;
            if(angularVelocity.Y < -initialFriction) friction.Y -= initialFriction;
            if(angularVelocity.Z < -initialFriction) friction.Z -= initialFriction;

            if(angularVelocity.X > 0.0f)
                friction.X += angularVelocity.X * angularVelocity.X / m_angularFrictionTimescale.X;
            else
                friction.X -= angularVelocity.X * angularVelocity.X / m_angularFrictionTimescale.X;

            if(angularVelocity.Y > 0.0f)
                friction.Y += angularVelocity.Y * angularVelocity.Y / m_angularFrictionTimescale.Y;
            else
                friction.Y -= angularVelocity.Y * angularVelocity.Y / m_angularFrictionTimescale.Y;

            if(angularVelocity.Z > 0.0f)
                friction.Z += angularVelocity.Z * angularVelocity.Z / m_angularFrictionTimescale.Z;
            else
                friction.Z -= angularVelocity.Z * angularVelocity.Z / m_angularFrictionTimescale.Z;

            if(Math.Abs(m_angularMotorDirection.X) > 0.01f) friction.X = 0.0f;
            if(Math.Abs(m_angularMotorDirection.Y) > 0.01f) friction.Y = 0.0f;
            if(Math.Abs(m_angularMotorDirection.Z) > 0.01f) friction.Z = 0.0f;

            #endregion

            #region Vertical attraction

            if(m_verticalAttractionTimescale < 300)
            {
                float VAservo = 38.0f / m_verticalAttractionTimescale;
                if(Type == Vehicle.TYPE_CAR) VAservo = 10.0f / m_verticalAttractionTimescale;

                Vector3 verterr = new Vector3(0.0f, 0.0f, 1.0f);
                verterr *= rotq;
                vertattr.X = verterr.Y;
                vertattr.Y = -verterr.X;
                vertattr.Z = 0.0f;

                vertattr *= irotq;

                //when upsidedown prefer x rotation of body, to keep forward movement direction the same
                if(verterr.Z < 0.0f) {
                    vertattr.Y = -vertattr.Y * 2.0f;
                    if(vertattr.X < 0.0f) vertattr.X = -2.0f - vertattr.X;
                    else vertattr.X = 2.0f - vertattr.X;
                }

                vertattr *= VAservo;

                vertattr.X += (vertattr.X - angularVelocity.X) * (0.004f * m_verticalAttractionEfficiency + 0.0001f);
                vertattr.Y += (vertattr.Y - angularVelocity.Y) * (0.004f * m_verticalAttractionEfficiency + 0.0001f);

                if((m_flags & (VehicleFlag.LIMIT_ROLL_ONLY)) != 0) vertattr.Y = 0.0f;
            }

            #endregion

            #region deflection
            //rotates body to direction of movement (linearMovement vector)
            /* temporary disabled due to instabilities, needs to be rewritten
            if(m_angularDeflectionTimescale < 300)
            {
                float Dservo = 0.05f * m_angularDeflectionTimescale * m_angularDeflectionEfficiency;
                float mag = (float)linearVelocity.LengthSquared();
                if(mag > 0.01f) {
                    linearVelocity.Y = -linearVelocity.Y;
                    linearVelocity *= rotq;

                    mag = (float)Math.Sqrt(mag);

                    linearVelocity.Y /= mag;
                    linearVelocity.Z /= mag;

                    deflection.Y = -linearVelocity.Z;
                    deflection.Z = -linearVelocity.Y;

                    deflection *= Dservo;
                }
            }
            */
            #endregion

            #region banking

            if(m_verticalAttractionTimescale < 300 && m_bankingEfficiency > 0) { //vertical attraction must be enabled
                float mag = (float)(linearVelocity.X * linearVelocity.X + linearVelocity.Y * linearVelocity.Y);
                if(mag > 0.01f) {
                    mag = (float)Math.Sqrt(mag);
                    if(mag > 20.0f) mag = 1.0f;
                    else mag /= 20.0f;
                }
                else mag = 0.0f;

                float b_static = -m_angularMotorDirection.X * 0.12f * (1.0f - m_bankingMix);
                float b_dynamic = -m_angularMotorDirection.X * 0.12f * mag * m_bankingMix;

                banking.Z = (b_static + b_dynamic - d_angularVelocity.Z) / m_bankingTimescale * m_bankingEfficiency;
            }

            #endregion

            m_lastAngularVelocity = angularVelocity;

            if(parent.LinkSetIsColliding || Type == Vehicle.TYPE_AIRPLANE || Type == Vehicle.TYPE_BALLOON || ishovering) {
                angularVelocity += deflection;
                angularVelocity -= friction;
            }
            else {
                banking = Vector3.Zero;
            }

            angularVelocity += m_angularMotorVelocity;
            angularVelocity += vertattr;
            angularVelocity *= rotq;
            angularVelocity += banking;

            if(angularVelocity.LengthSquared() < 1e-5f) {
                angularVelocity = Vector3.Zero;
                d.BodySetAngularVel(Body, 0, 0, 0);
                m_angularZeroFlag = true;
            }
            else {
                d.BodySetAngularVel(Body, angularVelocity.X, angularVelocity.Y, angularVelocity.Z);
                m_angularZeroFlag = false;
            }
        }
        internal void Step(IntPtr pBody, float pTimestep, UniverseODEPhysicsScene pParentScene, UniverseODEPrim parent)
        {
            m_body = pBody;
            if (pBody == IntPtr.Zero || m_type == Vehicle.TYPE_NONE)
                return;
            if (Mass == 0)
                GetMass(pBody);
            if (Mass == 0)
                return; //No noMass vehicles...
            if (!d.BodyIsEnabled(Body))
                d.BodyEnable(Body);

            frcount++; // used to limit debug comment output
            if (frcount > 100)
                frcount = 0;

            // scale time so parameters work as before
            // until we scale then acording to ode step time

            MoveLinear(pTimestep, pParentScene, parent);
            MoveAngular(pTimestep, pParentScene, parent);
            LimitRotation(pTimestep);
        }
        //end ProcessVehicleFlags
        internal void ProcessTypeChange(UniverseODEPrim parent, Vehicle pType, float timestep)
        {
            // Set Defaults For Type
            m_type = pType;
            switch (pType)
            {
                case Vehicle.TYPE_NONE:
                    m_linearFrictionTimescale = new Vector3(1000/timestep, 1000/timestep, 1000/timestep);
                    m_angularFrictionTimescale = new Vector3(1000/timestep, 1000/timestep, 1000/timestep);
                    m_linearMotorDirection = Vector3.Zero;
                    m_linearMotorTimescale = 1000/timestep;
                    m_linearMotorDecayTimescale = 1000/timestep;
                    m_angularMotorDirection = Vector3.Zero;
                    m_angularMotorTimescale = 1000/timestep;
                    m_angularMotorDecayTimescale = 120/timestep;
                    m_VhoverHeight = 0;
                    m_VhoverTimescale = 1000/timestep;
                    m_VehicleBuoyancy = 0;

                    m_linearDeflectionEfficiency = 1;
                    m_linearDeflectionTimescale = 1/timestep;

                    m_angularDeflectionEfficiency = 0;
                    m_angularDeflectionTimescale = 1000/timestep;

                    m_bankingEfficiency = 0;
                    m_bankingMix = 1;
                    m_bankingTimescale = 1000/timestep;

                    m_flags = 0;
                    m_referenceFrame = Quaternion.Identity;
                    break;

                case Vehicle.TYPE_SLED:
                    m_linearFrictionTimescale = new Vector3(30/timestep, 1/timestep, 1000/timestep);

                    m_angularFrictionTimescale = new Vector3(1000/timestep, 1000/timestep, 1000/timestep);

                    m_linearMotorDirection = Vector3.Zero;
                    m_linearMotorTimescale = 1000/timestep;
                    m_linearMotorDecayTimescale = 120/timestep;

                    m_angularMotorDirection = Vector3.Zero;
                    m_angularMotorTimescale = 1000/timestep;
                    m_angularMotorDecayTimescale = 120/timestep;

                    m_VhoverHeight = 0;
                    m_VhoverEfficiency = 10;
                    m_VhoverTimescale = 10/timestep;
                    m_VehicleBuoyancy = 0;

                    m_linearDeflectionEfficiency = 1;
                    m_linearDeflectionTimescale = 1/timestep;

                    m_angularDeflectionEfficiency = 0;
                    m_angularDeflectionTimescale = 1000/timestep;

                    m_bankingEfficiency = 0;
                    m_bankingMix = 1;
                    m_bankingTimescale = 10/timestep;

                    m_referenceFrame = Quaternion.Identity;
                    m_flags &=
                        ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
                          VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
                    break;
                case Vehicle.TYPE_CAR:
                    m_linearFrictionTimescale = new Vector3(100/timestep, 2/timestep, 1000/timestep);
                    m_angularFrictionTimescale = new Vector3(1000/timestep, 1000/timestep, 1000/timestep);
                    m_linearMotorDirection = Vector3.Zero;
                    m_linearMotorTimescale = 1/timestep;
                    m_linearMotorDecayTimescale = 60/timestep;
                    m_angularMotorDirection = Vector3.Zero;
                    m_angularMotorTimescale = 1/timestep;
                    m_angularMotorDecayTimescale = 0.8f/timestep;
                    m_VhoverHeight = 0;
                    m_VhoverEfficiency = 0;
                    m_VhoverTimescale = 1000/timestep;
                    m_VehicleBuoyancy = 0;
                    m_linearDeflectionEfficiency = 1;
                    m_linearDeflectionTimescale = 2/timestep;
                    m_angularDeflectionEfficiency = 0;
                    m_angularDeflectionTimescale = 10/timestep;
                    m_verticalAttractionEfficiency = 1f;
                    m_verticalAttractionTimescale = 10f/timestep;
                    m_bankingEfficiency = -0.2f;
                    m_bankingMix = 1;
                    m_bankingTimescale = 1/timestep;
                    m_referenceFrame = Quaternion.Identity;
                    m_flags &=
                        ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
                          VehicleFlag.HOVER_GLOBAL_HEIGHT);
                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY |
                                VehicleFlag.LIMIT_MOTOR_UP | VehicleFlag.HOVER_UP_ONLY);
                    break;
                case Vehicle.TYPE_BOAT:
                    m_linearFrictionTimescale = new Vector3(10/timestep, 3/timestep, 2/timestep);
                    m_angularFrictionTimescale = new Vector3(10/timestep, 10/timestep, 10/timestep);
                    m_linearMotorDirection = Vector3.Zero;
                    m_linearMotorTimescale = 5/timestep;
                    m_linearMotorDecayTimescale = 60/timestep;
                    m_angularMotorDirection = Vector3.Zero;
                    m_angularMotorTimescale = 4/timestep;
                    m_angularMotorDecayTimescale = 4/timestep;
                    m_VhoverHeight = 0;
                    m_VhoverEfficiency = 0.5f;
                    m_VhoverTimescale = 2/timestep;
                    m_VehicleBuoyancy = 1;
                    m_linearDeflectionEfficiency = 0.5f;
                    m_linearDeflectionTimescale = 3/timestep;
                    m_angularDeflectionEfficiency = 0.5f;
                    m_angularDeflectionTimescale = 5/timestep;
                    m_verticalAttractionEfficiency = 0.5f;
                    m_verticalAttractionTimescale = 5f/timestep;
                    m_bankingEfficiency = -0.3f;
                    m_bankingMix = 0.8f;
                    m_bankingTimescale = 1/timestep;
                    m_referenceFrame = Quaternion.Identity;
                    m_flags &= ~(VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
                                 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
                                VehicleFlag.LIMIT_MOTOR_UP |
                                VehicleFlag.HOVER_WATER_ONLY);
                    break;
                case Vehicle.TYPE_AIRPLANE:
                    m_linearFrictionTimescale = new Vector3(200/timestep, 10/timestep, 5/timestep);
                    m_angularFrictionTimescale = new Vector3(20/timestep, 20/timestep, 20/timestep);
                    m_linearMotorDirection = Vector3.Zero;
                    m_linearMotorTimescale = 2/timestep;
                    m_linearMotorDecayTimescale = 60/timestep;
                    m_angularMotorDirection = Vector3.Zero;
                    m_angularMotorTimescale = 4/timestep;
                    m_angularMotorDecayTimescale = 4/timestep;
                    m_VhoverHeight = 0;
                    m_VhoverEfficiency = 0.5f;
                    m_VhoverTimescale = 1000/timestep;
                    m_VehicleBuoyancy = 0;
                    m_linearDeflectionEfficiency = 0.5f;
                    m_linearDeflectionTimescale = 3/timestep;
                    m_angularDeflectionEfficiency = 1;
                    m_angularDeflectionTimescale = 2/timestep;
                    m_verticalAttractionEfficiency = 0.9f;
                    m_verticalAttractionTimescale = 2f/timestep;
                    m_bankingEfficiency = 1;
                    m_bankingMix = 0.7f;
                    m_bankingTimescale = 2;
                    m_referenceFrame = Quaternion.Identity;
                    m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_MOTOR_UP |
                                 VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
                                 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
                    m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
                    break;
                case Vehicle.TYPE_BALLOON:
                    m_linearFrictionTimescale = new Vector3(5/timestep, 5/timestep, 5/timestep);
                    m_angularFrictionTimescale = new Vector3(10/timestep, 10/timestep, 10/timestep);
                    m_linearMotorDirection = Vector3.Zero;
                    m_linearMotorTimescale = 5/timestep;
                    m_linearMotorDecayTimescale = 60/timestep;
                    m_angularMotorDirection = Vector3.Zero;
                    m_angularMotorTimescale = 6/timestep;
                    m_angularMotorDecayTimescale = 10/timestep;
                    m_VhoverHeight = 5;
                    m_VhoverEfficiency = 0.8f;
                    m_VhoverTimescale = 10/timestep;
                    m_VehicleBuoyancy = 1;
                    m_linearDeflectionEfficiency = 0;
                    m_linearDeflectionTimescale = 5/timestep;
                    m_angularDeflectionEfficiency = 0;
                    m_angularDeflectionTimescale = 5/timestep;
                    m_verticalAttractionEfficiency = 1f;
                    m_verticalAttractionTimescale = 100f/timestep;
                    m_bankingEfficiency = 0;
                    m_bankingMix = 0.7f;
                    m_bankingTimescale = 5/timestep;
                    m_referenceFrame = Quaternion.Identity;
                    m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_MOTOR_UP |
                                 VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
                                 VehicleFlag.HOVER_UP_ONLY);
                    m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
                    break;
            }
        }
 //end SetDefaultsForType
 internal void Enable(IntPtr pBody, UniverseODEPrim parent, UniverseODEPhysicsScene pParentScene)
 {
     if (m_enabled)
         return;
     if (pBody == IntPtr.Zero || m_type == Vehicle.TYPE_NONE)
         return;
     m_body = pBody;
     d.BodySetGravityMode(Body, true);
     m_enabled = true;
     m_lastLinearVelocityVector = parent.Velocity;
     m_lastPositionVector = parent.Position;
     m_lastAngularVelocity = parent.RotationalVelocity;
     parent.ThrottleUpdates = false;
     GetMass(pBody);
 }
 internal void Disable(UniverseODEPrim parent)
 {
     if (!m_enabled || m_type == Vehicle.TYPE_NONE)
         return;
     m_enabled = false;
     parent.ThrottleUpdates = true;
     parent.ForceSetVelocity(Vector3.Zero);
     parent.ForceSetRotVelocity(Vector3.Zero);
     parent.ForceSetPosition(parent.Position);
     d.BodySetGravityMode(Body, false);
     m_body = IntPtr.Zero;
     m_linearMotorDirection = Vector3.Zero;
     m_linearMotorDirectionLASTSET = Vector3.Zero;
     m_angularMotorDirection = Vector3.Zero;
 }