private void changelink(AuroraODEPrim 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 AuroraODEPrim)
                {
                    if (newparent != _parent)
                    {
                        AuroraODEPrim obj = (AuroraODEPrim) _parent;
                        obj.ChildDelink(this);
                        childPrim = false;

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

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

            DestroyBody();

            if (odePrim == this)
            {
                AuroraODEPrim newroot = null;
                lock (childrenPrim)
                {
                    if (childrenPrim.Count > 0)
                    {
                        newroot = childrenPrim[0];
                        childrenPrim.RemoveAt(0);
                        foreach (AuroraODEPrim 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();
        }
        public override PhysicsActor AddPrimShape(UUID primID, uint localID, string name, byte physicsType, PrimitiveBaseShape shape, Vector3 position,
                                                    Vector3 size, Quaternion rotation, bool isPhysical, int material, float friction, float restitution,
                                                    float gravityMultiplier, float density)
        {
            AuroraODEPrim newPrim = new AuroraODEPrim(name, physicsType, shape, position, size, rotation, material, friction, restitution, gravityMultiplier, density, this);
            newPrim.UUID = primID;
            newPrim.LocalID = localID;

            if (isPhysical)
                newPrim.IsPhysical = isPhysical;

            lock (_prims)
                _prims.Add(newPrim);

            return newPrim;
        }
        // I'm the parent
        // prim is the child
        public void ParentPrim(AuroraODEPrim prim)
        {
            //Console.WriteLine("ParentPrim  " + m_primName);
            if (this.m_localID != prim.m_localID)
            {
                DestroyBody();

                lock (childrenPrim)
                {
                    foreach (AuroraODEPrim 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
            }
        }
 internal void remActivePrim(AuroraODEPrim deactivatePrim)
 {
     lock (_activeprimsLock)
         _activeprims.Remove(deactivatePrim);
 }
 /// <summary>
 ///     This is called from within simulate but outside the locked portion
 ///     We need to do our own locking here
 ///     Essentially, we need to remove the prim from our space segment, whatever segment it's in.
 ///     If there are no more prim in the segment, we need to empty (spacedestroy)the segment and reclaim memory
 ///     that the space was using.
 /// </summary>
 /// <param name="prim"></param>
 internal void RemovePrimThreadLocked(AuroraODEPrim prim)
 {
     remCollisionEventReporting(prim);
     remActivePrim(prim);
     prim.m_frozen = true;
     if (prim.prim_geom != IntPtr.Zero)
     {
         prim.DestroyBody();
         prim.IsPhysical = false;
         prim.m_targetSpace = IntPtr.Zero;
         try
         {
             if (prim.prim_geom != IntPtr.Zero)
             {
                 d.GeomDestroy(prim.prim_geom);
                 prim.prim_geom = IntPtr.Zero;
             }
             else
             {
                 MainConsole.Instance.Warn("[PHYSICS]: Unable to remove prim from physics scene");
             }
         }
         catch (AccessViolationException)
         {
             MainConsole.Instance.Info(
                 "[PHYSICS]: Couldn't remove prim from physics scene, it was already be removed.");
         }
     }
     if (!prim.childPrim)
     {
         lock (prim.childrenPrim)
             foreach (AuroraODEPrim prm in prim.childrenPrim)
                 RemovePrimThreadLocked(prm);
     }
     lock (_prims)
         _prims.Remove(prim);
 }
 internal void BadPrim(AuroraODEPrim auroraODEPrim)
 {
     DeletePrim(auroraODEPrim);
     //Can't really do this here... as it will be readded before the delete gets called, which is wrong...
     //So... leave the prim out there for now
     //AddPrimShape(auroraODEPrim.ParentEntity);
 }
 //end SetDefaultsForType
 internal void Enable(IntPtr pBody, AuroraODEPrim parent, AuroraODEPhysicsScene 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);
 }
        // end Step
        private void MoveLinear(float pTimestep, AuroraODEPhysicsScene _pParentScene, AuroraODEPrim parent)
        {
            if (m_linearMotorDirection.LengthSquared() < 0.0001f)
            {
                m_linearMotorDirection = Vector3.Zero;
                m_newVelocity = Vector3.Zero;
            }
            else
            {
                Vector3 addAmount = (m_linearMotorDirection - m_lastLinearVelocityVector)/(m_linearMotorTimescale);
                m_lastLinearVelocityVector += (addAmount);

                m_linearMotorDirection *= (1.0f - 1.0f/m_linearMotorDecayTimescale);

                // convert requested object velocity to world-referenced vector
                d.Quaternion rot = d.BodyGetQuaternion(Body);
                Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
                //Vector3 oldVelocity = m_newVelocity;
                m_newVelocity = m_lastLinearVelocityVector*rotq; // apply obj rotation to velocity vector
                //if (oldVelocity.Z == 0 && (Type != Vehicle.TYPE_AIRPLANE && Type != Vehicle.TYPE_BALLOON))
                //    m_newVelocity.Z += dvel_now.Z; // Preserve the accumulated falling velocity
            }

            //if (m_newVelocity.Z == 0 && (Type != Vehicle.TYPE_AIRPLANE && Type != Vehicle.TYPE_BALLOON))
            //    m_newVelocity.Z += dvel_now.Z; // Preserve the accumulated falling velocity

            d.Vector3 dpos = d.BodyGetPosition(Body);
            Vector3 pos = new Vector3(dpos.X, dpos.Y, dpos.Z);

            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;
                //Make sure that we don't have an explosion the next frame with the posChange
                d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
            }
            else if (pos.Z < terrainHeight)
                m_newVelocity.Z += 1;

            #endregion

            #region Hover

            // Check if hovering
            if ((m_flags &
                 (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
            {
                // We should hover, get the target height
                if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0)
                {
                    m_VhoverTargetHeight = (float) _pParentScene.GetWaterLevel(pos.X, pos.Y) + m_VhoverHeight;
                }
                if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
                {
                    m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
                }
                if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0)
                {
                    m_VhoverTargetHeight = 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;
                }
                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
                {
                    float herr0 = pos.Z - tempHoverHeight;
                    // Replace Vertical speed with correction figure if significant
                    if (herr0 > 0.01f)
                    {
                        m_newVelocity.Z = -((herr0*50.0f)/m_VhoverTimescale);
                        //KF: m_VhoverEfficiency is not yet implemented
                    }
                    else if (herr0 < -0.01f)
                    {
                        m_newVelocity.Z = -((herr0*50f)/m_VhoverTimescale);
                    }
                    else
                    {
                        m_newVelocity.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
            }

            #endregion

            #region No X,Y,Z

            if ((m_flags & (VehicleFlag.NO_X)) != 0)
                m_newVelocity.X = 0;
            if ((m_flags & (VehicleFlag.NO_Y)) != 0)
                m_newVelocity.Y = 0;
            if ((m_flags & (VehicleFlag.NO_Z)) != 0)
                m_newVelocity.Z = 0;

            #endregion

            #region Deal with tainted forces

            // 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.
            // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
            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>();
            }

            // force to deltaV
            m_newVelocity += TaintedForce*(pTimestep/Mass);

            #endregion

            #region Deflection

            //Forward is the prefered direction
            /*Vector3 deflectionamount = m_newVelocity / (m_linearDeflectionTimescale / pTimestep);
            //deflectionamount *= m_linearDeflectionEfficiency;
            if (deflectionamount != Vector3.Zero)
            {
            }
            Vector3 deflection = Vector3.One / deflectionamount;
            m_newVelocity /= deflection;*/

            #endregion

            #region limitations

            if (m_newVelocity.LengthSquared() > 1e6f)
            {
                m_newVelocity /= m_newVelocity.Length();
                m_newVelocity *= 1000f;
            }
            else if (m_newVelocity.LengthSquared() < 1e-6f)
                m_newVelocity = Vector3.Zero;

            #endregion

            m_lastPositionVector = parent.Position;

            float grav = -1*Mass*pTimestep;

            // Apply velocity
            if (m_newVelocity != Vector3.Zero)
            {
                if ((Type == Vehicle.TYPE_CAR || Type == Vehicle.TYPE_SLED) && !parent.LinkSetIsColliding)
                {
                    //Force ODE gravity here!!!
                }
                else
                    d.BodySetLinearVel(Body, m_newVelocity.X, m_newVelocity.Y, m_newVelocity.Z + grav);
            }
            // apply friction
            m_lastLinearVelocityVector.X *= (1.0f - 1/m_linearFrictionTimescale.X);
            m_lastLinearVelocityVector.Y *= (1.0f - 1/m_linearFrictionTimescale.Y);
            m_lastLinearVelocityVector.Z *= (1.0f - 1/m_linearFrictionTimescale.Z);
        }
 internal void addActivePrim(AuroraODEPrim activatePrim)
 {
     // adds active prim..   (ones that should be iterated over in collisions_optimized
     lock (_activeprimsLock)
     {
         if (!_activeprims.Contains(activatePrim))
             _activeprims.Add(activatePrim);
     }
 }
        // end MoveLinear()
        private void MoveAngular(float pTimestep, AuroraODEPhysicsScene _pParentScene, AuroraODEPrim parent)
        {
            d.Vector3 angularVelocity = d.BodyGetAngularVel(Body);
            d.Quaternion rot = d.BodyGetQuaternion(Body);
            Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
            //         Vector3 angularVelocity = Vector3.Zero;

            /*if ((m_flags & VehicleFlag.MOUSELOOK_STEER) == VehicleFlag.MOUSELOOK_STEER)
            {
                if (m_userLookAt != Quaternion.Identity)
                {
                    Quaternion camrot = Quaternion.Subtract (m_userLookAt, rotq);
                    camrot.Normalize ();
                    m_angularMotorVelocity += Vector3.One * camrot;
                    Console.WriteLine (Vector3.One * camrot);
                }
            }*/

            if (m_angularMotorDirection.LengthSquared() > 1e-6f)
            {
                m_angularMotorVelocity.X = (m_angularMotorDirection.X - angularVelocity.X)/(m_angularMotorTimescale);
                m_angularMotorVelocity.Y = (m_angularMotorDirection.Y - angularVelocity.Y)/(m_angularMotorTimescale);
                m_angularMotorVelocity.Z = (m_angularMotorDirection.Z - angularVelocity.Z)/(m_angularMotorTimescale);
                m_angularMotorDirection *= (1.0f - 1.0f/m_angularMotorDecayTimescale);
            }
            else
            {
                m_angularMotorVelocity = Vector3.Zero;
            } // end motor section

            // Vertical attractor section
            Vector3 vertattr = Vector3.Zero;
            Vector3 deflection = Vector3.Zero;
            Vector3 banking = Vector3.Zero;

            if (m_verticalAttractionTimescale < 300 && m_lastAngularVelocity != Vector3.Zero)
            {
                float VAservo = 0;
                if (Type == Vehicle.TYPE_BOAT)
                {
                    VAservo = 0.2f/(m_verticalAttractionTimescale);
                    VAservo *= (m_verticalAttractionEfficiency*m_verticalAttractionEfficiency);
                }
                else
                {
                    if (parent.LinkSetIsColliding)
                        VAservo = 0.05f/(m_verticalAttractionTimescale);
                    else
                        VAservo = 0.2f/(m_verticalAttractionTimescale);
                    VAservo *= (m_verticalAttractionEfficiency*m_verticalAttractionEfficiency);
                }
                // make a vector pointing up
                Vector3 verterr = Vector3.Zero;
                verterr.Z = 1.0f;
                // rotate it to Body Angle
                verterr = verterr*rotq;
                // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1.
                // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go
                // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
                if (verterr.Z < 0.0f)
                {
                    verterr.X = 2.0f - verterr.X;
                    verterr.Y = 2.0f - verterr.Y;
                }
                // Error is 0 (no error) to +/- 2 (max error)
                // scale it by VAservo
                verterr = verterr*VAservo;
                //if (frcount == 0) Console.WriteLine("VAerr=" + verterr);

                // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
                // Change  Body angular velocity  X based on Y, and Y based on X. Z is not changed.
                vertattr.X = verterr.Y;
                vertattr.Y = -verterr.X;
                vertattr.Z = 0f;

                // scaling appears better usingsquare-law
                float bounce = 1.0f - (m_verticalAttractionEfficiency*m_verticalAttractionEfficiency);
                vertattr.X += bounce*angularVelocity.X;
                vertattr.Y += bounce*angularVelocity.Y;
            } // else vertical attractor is off

            #region Deflection

            //Forward is the prefered direction, but if the reference frame has changed, we need to take this into account as well
            Vector3 PreferredAxisOfMotion =
                new Vector3((10*(m_angularDeflectionEfficiency/m_angularDeflectionTimescale)), 0, 0);
            PreferredAxisOfMotion *= Quaternion.Add(rotq, m_referenceFrame);

            //Multiply it so that it scales linearly
            //deflection = PreferredAxisOfMotion;

            //deflection = ((PreferredAxisOfMotion * m_angularDeflectionEfficiency) / (m_angularDeflectionTimescale / pTimestep));

            #endregion

            #region Banking

            if (m_bankingEfficiency != 0)
            {
                Vector3 dir = Vector3.One*rotq;
                float mult = (m_bankingMix*m_bankingMix)*-1*(m_bankingMix < 0 ? -1 : 1);
                //Changes which way it banks in and out of turns

                //Use the square of the efficiency, as it looks much more how SL banking works
                float effSquared = (m_bankingEfficiency*m_bankingEfficiency);
                if (m_bankingEfficiency < 0)
                    effSquared *= -1; //Keep the negative!

                float mix = Math.Abs(m_bankingMix);
                if (m_angularMotorVelocity.X == 0)
                {
                    /*if (!parent.Orientation.ApproxEquals(this.m_referenceFrame, 0.25f))
                    {
                        Vector3 axisAngle;
                        float angle;
                        parent.Orientation.GetAxisAngle(out axisAngle, out angle);
                        Vector3 rotatedVel = parent.Velocity * parent.Orientation;
                        if ((rotatedVel.X < 0 && axisAngle.Y > 0) || (rotatedVel.X > 0 && axisAngle.Y < 0))
                            m_angularMotorVelocity.X += (effSquared * (mult * mix)) * (1f) * 10;
                        else
                            m_angularMotorVelocity.X += (effSquared * (mult * mix)) * (-1f) * 10;
                    }*/
                }
                else
                    banking.Z += (effSquared*(mult*mix))*(m_angularMotorVelocity.X)*4;
                if (!parent.LinkSetIsColliding && Math.Abs(m_angularMotorVelocity.X) > mix)
                    //If they are colliding, we probably shouldn't shove the prim around... probably
                {
                    float angVelZ = m_angularMotorVelocity.X*-1;
                    /*if(angVelZ > mix)
                        angVelZ = mix;
                    else if(angVelZ < -mix)
                        angVelZ = -mix;*/
                    //This controls how fast and how far the banking occurs
                    Vector3 bankingRot = new Vector3(angVelZ*(effSquared*mult), 0, 0);
                    if (bankingRot.X > 3)
                        bankingRot.X = 3;
                    else if (bankingRot.X < -3)
                        bankingRot.X = -3;
                    bankingRot *= rotq;
                    banking += bankingRot;
                }
                m_angularMotorVelocity.X *= m_bankingEfficiency == 1 ? 0.0f : 1 - m_bankingEfficiency;
            }

            #endregion

            #region Downward Force

            Vector3 downForce = Vector3.Zero;

            double Zchange = m_lastposChange.Z;
            if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0) //if it isn't going up, don't apply the limiting force
            {
                if (Zchange < -0.1f)
                {
                    if (Zchange < -0.3f)
                        Zchange = -0.3f;
                    //Requires idea of 'up', so use reference frame to rotate it
                    //Add to the X, because that will normally tilt the vehicle downward (if its rotated, it'll be rotated by the ref. frame
                    downForce = (new Vector3(0, ((float) Math.Abs(Zchange)*(pTimestep*_pParentScene.PID_P/4)), 0));
                    downForce *= rotq;
                }
            }

            #endregion

            // Sum velocities
            m_lastAngularVelocity = m_angularMotorVelocity + vertattr + deflection + banking + downForce;

            #region Linear Motor Offset

            //Offset section
            if (m_linearMotorOffset != Vector3.Zero)
            {
                //Offset of linear velocity doesn't change the linear velocity,
                //   but causes a torque to be applied, for example...
                //
                //      IIIII     >>>   IIIII
                //      IIIII     >>>    IIIII
                //      IIIII     >>>     IIIII
                //          ^
                //          |  Applying a force at the arrow will cause the object to move forward, but also rotate
                //
                //
                // The torque created is the linear velocity crossed with the offset

                //Note: we use the motor, otherwise you will just spin around and we divide by 10 since otherwise we go crazy
                Vector3 torqueFromOffset = (m_linearMotorDirectionLASTSET/m_linearMotorOffset);
                if (float.IsNaN(torqueFromOffset.X))
                    torqueFromOffset.X = 0;
                if (float.IsNaN(torqueFromOffset.Y))
                    torqueFromOffset.Y = 0;
                if (float.IsNaN(torqueFromOffset.Z))
                    torqueFromOffset.Z = 0;
                d.BodyAddTorque(Body, torqueFromOffset.X, torqueFromOffset.Y, torqueFromOffset.Z);
            }

            #endregion

            /*if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
            {
                m_lastAngularVelocity.X = 0;
                m_lastAngularVelocity.Y = 0;
            }*/

            // apply friction

            // Apply to the body

            if (m_lastAngularVelocity.LengthSquared() < 0.0001f)
            {
                m_lastAngularVelocity = Vector3.Zero;
                d.BodySetAngularVel(Body, 0, 0, 0);
                m_angularZeroFlag = true;
            }
            else
            {
                if (!d.BodyIsEnabled(Body))
                    d.BodyEnable(Body);
                d.BodySetAngularVel(Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
                m_angularZeroFlag = false;

                m_lastAngularVelocity.X *= (1.0f - 1.0f/m_angularFrictionTimescale.X);
                m_lastAngularVelocity.Y *= (1.0f - 1.0f/m_angularFrictionTimescale.Y);
                m_lastAngularVelocity.Z *= (1.0f - 1.0f/m_angularFrictionTimescale.Z);
            }
        }
        internal void Step(IntPtr pBody, float pTimestep, AuroraODEPhysicsScene pParentScene, AuroraODEPrim 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(AuroraODEPrim 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;
            }
        }
 private void ChildSetGeom(AuroraODEPrim odePrim)
 {
     DestroyBody();
     MakeBody();
 }
        /// <summary>
        ///     Routine to figure out if we need to mesh this prim with our mesher
        /// </summary>
        /// <param name="entity"></param>
        /// <returns></returns>
        internal bool needsMeshing(AuroraODEPrim prim, byte physicalType)
        {
            PrimitiveBaseShape pbs = prim.Shape;
            // most of this is redundant now as the mesher will return null if it cant mesh a prim
            // but we still need to check for sculptie meshing being enabled so this is the most
            // convenient place to do it for now...

            //    //if (pbs.PathCurve == (byte)Primitive.PathCurve.Circle && pbs.ProfileCurve == (byte)Primitive.ProfileCurve.Circle && pbs.PathScaleY <= 0.75f)
            //    //MainConsole.Instance.Debug("needsMeshing: " + " pathCurve: " + pbs.PathCurve.ToString() + " profileCurve: " + pbs.ProfileCurve.ToString() + " pathScaleY: " + Primitive.UnpackPathScale(pbs.PathScaleY).ToString());
            int iPropertiesNotSupportedDefault = 0;

            //            return true;

            if (forceSimplePrimMeshing)
                return true;
            // let simple spheres use ode sphere object
            PrimitiveBaseShape sphere = PrimitiveBaseShape.CreateSphere();
            if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte) Extrusion.Curve1
                && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.X == pbs.Scale.Z && pbs.ProfileHollow == sphere.ProfileHollow &&
                pbs.PathBegin == sphere.PathBegin && pbs.PathEnd == sphere.PathEnd &&
                pbs.PathCurve == sphere.PathCurve && pbs.HollowShape == sphere.HollowShape &&
                pbs.PathRadiusOffset == sphere.PathRadiusOffset && pbs.PathRevolutions == sphere.PathRevolutions &&
                pbs.PathScaleY == sphere.PathScaleY && pbs.PathShearX == sphere.PathShearX &&
                pbs.PathShearY == sphere.PathShearY && pbs.PathSkew == sphere.PathSkew &&
                pbs.PathTaperY == sphere.PathTaperY && pbs.PathTwist == sphere.PathTwist &&
                pbs.PathTwistBegin == sphere.PathTwistBegin && pbs.ProfileBegin == sphere.ProfileBegin &&
                pbs.ProfileEnd == sphere.ProfileEnd && pbs.ProfileHollow == sphere.ProfileHollow &&
                pbs.ProfileShape == sphere.ProfileShape)
                return false;

            if (pbs.SculptEntry && !meshSculptedPrim)
                return false;
            else if (pbs.SculptType != (byte) SculptType.Mesh &&
                     pbs.SculptType != (byte) SculptType.None)
                return true; //Sculpty, mesh it
            else if (pbs.SculptType == (byte) SculptType.Mesh)
            {
                //Mesh, we need to see what the prims says to do with it
                if (physicalType == (byte) PhysicsShapeType.Prim)
                    return false; //Supposed to be a simple box, nothing more
                else
                    return true; //Mesh it!
            }

            // if it's a standard box or sphere with no cuts, hollows, twist or top shear, return false since ODE can use an internal representation for the prim
            if (!forceSimplePrimMeshing)
            {
                if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte) Extrusion.Straight)
                    /*|| (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1
                    && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z)*/)
                {
                    if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0
                        && pbs.ProfileHollow == 0
                        && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0
                        && pbs.PathBegin == 0 && pbs.PathEnd == 0
                        && pbs.PathTaperX == 0 && pbs.PathTaperY == 0
                        && pbs.PathScaleX == 100 && pbs.PathScaleY == 100
                        && pbs.PathShearX == 0 && pbs.PathShearY == 0)
                    {
            #if SPAM
                    MainConsole.Instance.Warn("NonMesh");
            #endif
                        return false;
                    }
                }
            }

            if (pbs.ProfileHollow != 0)
                iPropertiesNotSupportedDefault++;
            else if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0))
                iPropertiesNotSupportedDefault++;
            else if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0)
                iPropertiesNotSupportedDefault++;
            else if (pbs.PathBegin != 0 || pbs.PathEnd != 0)
                iPropertiesNotSupportedDefault++;
            else if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100))
                iPropertiesNotSupportedDefault++;
            else if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0))
                iPropertiesNotSupportedDefault++;
            else if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte) Extrusion.Straight)
                iPropertiesNotSupportedDefault++;
            else if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte) Extrusion.Curve1 &&
                     (pbs.Scale.X != pbs.Scale.Y || pbs.Scale.Y != pbs.Scale.Z || pbs.Scale.Z != pbs.Scale.X))
                iPropertiesNotSupportedDefault++;
            else if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte) Extrusion.Curve1)
                iPropertiesNotSupportedDefault++;
                // test for torus
            else if ((pbs.ProfileCurve & 0x07) == (byte) ProfileShape.Square &&
                     pbs.PathCurve == (byte) Extrusion.Curve1)
                iPropertiesNotSupportedDefault++;
            else if ((pbs.ProfileCurve & 0x07) == (byte) ProfileShape.HalfCircle &&
                     (pbs.PathCurve == (byte) Extrusion.Curve1 || pbs.PathCurve == (byte) Extrusion.Curve2))
                iPropertiesNotSupportedDefault++;
            else if ((pbs.ProfileCurve & 0x07) == (byte) ProfileShape.EquilateralTriangle)
            {
                if (pbs.PathCurve == (byte) Extrusion.Straight)
                {
                    iPropertiesNotSupportedDefault++;
                }
                else if (pbs.PathCurve == (byte) Extrusion.Curve1)
                {
                    iPropertiesNotSupportedDefault++;
                }
            }
            if ((pbs.ProfileCurve & 0x07) == (byte) ProfileShape.Circle)
            {
                if (pbs.PathCurve == (byte) Extrusion.Straight)
                {
                    iPropertiesNotSupportedDefault++;
                }

                    // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits
                else if (pbs.PathCurve == (byte) Extrusion.Curve1)
                {
                    iPropertiesNotSupportedDefault++;
                }
            }

            if (iPropertiesNotSupportedDefault == 0)
            {
            #if SPAM
                MainConsole.Instance.Warn("NonMesh");
            #endif
                return false;
            }
            #if SPAM
            MainConsole.Instance.Debug("Mesh");
            #endif
            return true;
        }
 private void SetInStaticSpace(AuroraODEPrim 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);
 }
 internal void Disable(AuroraODEPrim 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;
 }