1 files changed, 2131 insertions, 2131 deletions

diff --git a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPrim.cs b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPrim.cs
index 3e26456..6fb82ef 100644
--- a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPrim.cs
+++ b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPrim.cs
@@ -1,2131 +1,2131 @@
-/*

- * Copyright (c) Contributors, http://opensimulator.org/

- * See CONTRIBUTORS.TXT for a full list of copyright holders.

- *

- * Redistribution and use in source and binary forms, with or without

- * modification, are permitted provided that the following conditions are met:

- *     * Redistributions of source code must retain the above copyright

- *       notice, this list of conditions and the following disclaimer.

- *     * Redistributions in binary form must reproduce the above copyright

- *       notice, this list of conditions and the following disclaimer in the

- *       documentation and/or other materials provided with the distribution.

- *     * Neither the name of the OpenSim Project nor the

- *       names of its contributors may be used to endorse or promote products

- *       derived from this software without specific prior written permission.

- *

- * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY

- * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED

- * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE

- * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY

- * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES

- * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;

- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND

- * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT

- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS

- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

- */

-using System;

-using System.Collections.Generic;

-using System.Reflection;

-using System.Runtime.InteropServices;

-using System.Threading;

-using log4net;

-using OpenMetaverse;

-using BulletDotNET;

-using OpenSim.Framework;

-using OpenSim.Region.Physics.Manager;

-

-namespace OpenSim.Region.Physics.BulletDotNETPlugin

-{

-    public class BulletDotNETPrim : PhysicsActor

-    {

-        private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);

-

-        private PhysicsVector _position;

-        private PhysicsVector _velocity;

-        private PhysicsVector _torque = new PhysicsVector(0, 0, 0);

-        private PhysicsVector m_lastVelocity = new PhysicsVector(0.0f, 0.0f, 0.0f);

-        private PhysicsVector m_lastposition = new PhysicsVector(0.0f, 0.0f, 0.0f);

-        private Quaternion m_lastorientation = new Quaternion();

-        private PhysicsVector m_rotationalVelocity;

-        private PhysicsVector _size;

-        private PhysicsVector _acceleration;

-        // private d.Vector3 _zeroPosition = new d.Vector3(0.0f, 0.0f, 0.0f);

-        private Quaternion _orientation;

-        private PhysicsVector m_taintposition;

-        private PhysicsVector m_taintsize;

-        private PhysicsVector m_taintVelocity = new PhysicsVector(0, 0, 0);

-        private PhysicsVector m_taintTorque = new PhysicsVector(0, 0, 0);

-        private Quaternion m_taintrot;

-        private PhysicsVector m_angularlock = new PhysicsVector(1f, 1f, 1f);

-        private PhysicsVector m_taintAngularLock = new PhysicsVector(1f, 1f, 1f);

-        private btGeneric6DofConstraint Amotor;

-

-        private PhysicsVector m_PIDTarget = new PhysicsVector(0, 0, 0);

-        private float m_PIDTau = 0f;

-        private float m_PIDHoverHeight = 0f;

-        private float m_PIDHoverTau = 0f;

-        private bool m_useHoverPID = false;

-        private PIDHoverType m_PIDHoverType = PIDHoverType.Ground;

-        private float m_targetHoverHeight = 0f;

-        private float m_groundHeight = 0f;

-        private float m_waterHeight = 0f;

-        private float PID_D = 35f;

-        private float PID_G = 25f;

-        private float m_tensor = 5f;

-        private int body_autodisable_frames = 20;

-        private IMesh primMesh = null;

-

-        private bool m_usePID = false;

-

-

-        private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom

-                                                        | CollisionCategories.Space

-                                                        | CollisionCategories.Body

-                                                        | CollisionCategories.Character

-                                                     );

-

-        private bool m_taintshape = false;

-        private bool m_taintPhysics = false;

-        private bool m_collidesLand = true;

-        private bool m_collidesWater = false;

-        public bool m_returnCollisions = false;

-

-        // Default we're a Geometry

-        private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);

-

-        // Default, Collide with Other Geometries, spaces and Bodies

-        private CollisionCategories m_collisionFlags = m_default_collisionFlags;

-

-        public bool m_taintremove = false;

-        public bool m_taintdisable = false;

-        public bool m_disabled = false;

-        public bool m_taintadd = false;

-        public bool m_taintselected = false;

-        public bool m_taintCollidesWater = false;

-

-        public uint m_localID = 0;

-

-        //public GCHandle gc;

-        private CollisionLocker ode;

-

-        private bool m_taintforce = false;

-        private bool m_taintaddangularforce = false;

-        private PhysicsVector m_force = new PhysicsVector(0.0f, 0.0f, 0.0f);

-        private List<PhysicsVector> m_forcelist = new List<PhysicsVector>();

-        private List<PhysicsVector> m_angularforcelist = new List<PhysicsVector>();

-

-        private IMesh _mesh;

-        private PrimitiveBaseShape _pbs;

-        private BulletDotNETScene _parent_scene;

-        public btCollisionShape prim_geom;

-        public IntPtr _triMeshData;

-

-        private PhysicsActor _parent = null;

-        private PhysicsActor m_taintparent = null;

-

-        private List<BulletDotNETPrim> childrenPrim = new List<BulletDotNETPrim>();

-

-        private bool iscolliding = false;

-        private bool m_isphysical = false;

-        private bool m_isSelected = false;

-

-        internal bool m_isVolumeDetect = false; // If true, this prim only detects collisions but doesn't collide actively

-

-        private bool m_throttleUpdates = false;

-        private int throttleCounter = 0;

-        public int m_interpenetrationcount = 0;

-        public float m_collisionscore = 0;

-        public int m_roundsUnderMotionThreshold = 0;

-        private int m_crossingfailures = 0;

-

-        public float m_buoyancy = 0f;

-

-        public bool outofBounds = false;

-        private float m_density = 10.000006836f; // Aluminum g/cm3;

-

-        public bool _zeroFlag = false;

-        private bool m_lastUpdateSent = false;

-

-

-        private String m_primName;

-        private PhysicsVector _target_velocity;

-

-        public int m_eventsubscription = 0;

-        private CollisionEventUpdate CollisionEventsThisFrame = null;

-

-        public volatile bool childPrim = false;

-

-        private btVector3 tempPosition1;

-        private btVector3 tempPosition2;

-        private btVector3 tempPosition3;

-        private btVector3 tempSize1;

-        private btVector3 tempSize2;

-        private btVector3 tempLinearVelocity1;

-        private btVector3 tempLinearVelocity2;

-        private btVector3 tempAngularVelocity1;

-        private btVector3 tempAngularVelocity2;

-        private btVector3 tempInertia1;

-        private btVector3 tempInertia2;

-        private btQuaternion tempOrientation1;

-        private btQuaternion tempOrientation2;

-        private btMotionState tempMotionState1;

-        private btMotionState tempMotionState2;

-        private btMotionState tempMotionState3;

-        private btTransform tempTransform1;

-        private btTransform tempTransform2;

-        private btTransform tempTransform3;

-        private btTransform tempTransform4;

-        private btTriangleIndexVertexArray btshapeArray;

-

-        public btRigidBody Body;

-

-        public BulletDotNETPrim(String primName, BulletDotNETScene parent_scene, PhysicsVector pos, PhysicsVector size,

-                       Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical)

-        {

-            tempPosition1 = new btVector3(0, 0, 0);

-            tempPosition2 = new btVector3(0, 0, 0);

-            tempPosition3 = new btVector3(0, 0, 0);

-            tempSize1 = new btVector3(0, 0, 0);

-            tempSize2 = new btVector3(0, 0, 0);

-            tempLinearVelocity1 = new btVector3(0, 0, 0);

-            tempLinearVelocity2 = new btVector3(0, 0, 0);

-            tempAngularVelocity1 = new btVector3(0, 0, 0);

-            tempAngularVelocity2 = new btVector3(0, 0, 0);

-            tempInertia1 = new btVector3(0, 0, 0);

-            tempInertia2 = new btVector3(0, 0, 0);

-            tempOrientation1 = new btQuaternion(0,0,0,1);

-            tempOrientation2 = new btQuaternion(0, 0, 0, 1);

-            _parent_scene = parent_scene;

-            tempTransform1 = new btTransform(_parent_scene.QuatIdentity, _parent_scene.VectorZero);

-            tempTransform2 = new btTransform(_parent_scene.QuatIdentity, _parent_scene.VectorZero); ;

-            tempTransform3 = new btTransform(_parent_scene.QuatIdentity, _parent_scene.VectorZero); ;

-            tempTransform4 = new btTransform(_parent_scene.QuatIdentity, _parent_scene.VectorZero); ;

-

-            tempMotionState1 = new btDefaultMotionState(_parent_scene.TransZero);

-            tempMotionState2 = new btDefaultMotionState(_parent_scene.TransZero);

-            tempMotionState3 = new btDefaultMotionState(_parent_scene.TransZero);

-

-            _target_velocity = new PhysicsVector(0, 0, 0);

-            _velocity = new PhysicsVector();

-            _position = pos;

-            m_taintposition = pos;

-            PID_D = parent_scene.bodyPIDD;

-            PID_G = parent_scene.bodyPIDG;

-            m_density = parent_scene.geomDefaultDensity;

-            m_tensor = parent_scene.bodyMotorJointMaxforceTensor;

-            body_autodisable_frames = parent_scene.bodyFramesAutoDisable;

-

-            prim_geom = null;

-            Body = null;

-

-            if (size.X <= 0) size.X = 0.01f;

-            if (size.Y <= 0) size.Y = 0.01f;

-            if (size.Z <= 0) size.Z = 0.01f;

-

-            _size = size;

-            m_taintsize = _size;

-            _acceleration = new PhysicsVector();

-            m_rotationalVelocity = PhysicsVector.Zero;

-            _orientation = rotation;

-            m_taintrot = _orientation;

-            _mesh = mesh;

-            _pbs = pbs;

-

-            _parent_scene = parent_scene;

-

-            if (pos.Z < 0)

-                m_isphysical = false;

-            else

-            {

-                m_isphysical = pisPhysical;

-                // If we're physical, we need to be in the master space for now.

-                // linksets *should* be in a space together..  but are not currently

-            }

-            m_primName = primName;

-            m_taintadd = true;

-            _parent_scene.AddPhysicsActorTaint(this);

-

-        }

-

-        #region PhysicsActor overrides

-

-        public override bool Stopped

-        {

-            get { return _zeroFlag; }

-        }

-

-        public override PhysicsVector Size

-        {

-            get { return _size; }

-            set { _size = value; }

-        }

-

-        public override PrimitiveBaseShape Shape

-        {

-            set

-            {

-                _pbs = value;

-                m_taintshape = true;

-            }

-        }

-

-        public override uint LocalID

-        {

-            set

-            {

-                //m_log.Info("[PHYSICS]: Setting TrackerID: " + value);

-                m_localID = value;

-            }

-        }

-

-        public override bool Grabbed

-        {

-            set { return; }

-        }

-

-        public override bool Selected

-        {

-            set

-            {

-                // This only makes the object not collidable if the object

-                // is physical or the object is modified somehow *IN THE FUTURE*

-                // without this, if an avatar selects prim, they can walk right

-                // through it while it's selected

-                m_collisionscore = 0;

-                if ((m_isphysical && !_zeroFlag) || !value)

-                {

-                    m_taintselected = value;

-                    _parent_scene.AddPhysicsActorTaint(this);

-                }

-                else

-                {

-                    m_taintselected = value;

-                    m_isSelected = value;

-                }

-            }

-        }

-

-        public override void CrossingFailure()

-        {

-            m_crossingfailures++;

-            if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds)

-            {

-                base.RaiseOutOfBounds(_position);

-                return;

-            }

-            else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds)

-            {

-                m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName);

-            }

-        }

-        public override void link(PhysicsActor obj)

-        {

-            //TODO:

-        }

-

-        public override void delink()

-        {

-            //TODO:

-        }

-

-        public override void LockAngularMotion(PhysicsVector axis)

-        {

-            m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);

-            m_taintAngularLock = new PhysicsVector(axis.X, axis.Y, axis.Z);

-        }

-

-        public override PhysicsVector Position

-        {

-            get { return _position; }

-

-            set

-            {

-                _position = value;

-                //m_log.Info("[PHYSICS]: " + _position.ToString());

-            }

-        }

-

-        public override float Mass

-        {

-            get { return CalculateMass(); }

-        }

-

-        public override PhysicsVector Force

-        {

-            //get { return PhysicsVector.Zero; }

-            get { return m_force; }

-            set { m_force = value; }

-        }

-

-        public override int VehicleType

-        {

-            get { return 0; }

-            set { return; }

-        }

-

-        public override void VehicleFloatParam(int param, float value)

-        {

-            //TODO:

-        }

-

-        public override void VehicleVectorParam(int param, PhysicsVector value)

-        {

-            //TODO:

-        }

-

-        public override void VehicleRotationParam(int param, Quaternion rotation)

-        {

-            //TODO:

-        }

-

-        public override void SetVolumeDetect(int param)

-        {

-            //TODO: GhostObject

-            m_isVolumeDetect = (param != 0);

-

-        }

-

-        public override PhysicsVector GeometricCenter

-        {

-            get { return PhysicsVector.Zero; }

-        }

-

-        public override PhysicsVector CenterOfMass

-        {

-            get { return PhysicsVector.Zero; }

-        }

-

-        public override PhysicsVector Velocity

-        {

-            get

-            {

-                // Averate previous velocity with the new one so

-                // client object interpolation works a 'little' better

-                PhysicsVector returnVelocity = new PhysicsVector();

-                returnVelocity.X = (m_lastVelocity.X + _velocity.X) / 2;

-                returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y) / 2;

-                returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z) / 2;

-                return returnVelocity;

-            }

-            set

-            {

-                _velocity = value;

-

-                m_taintVelocity = value;

-                _parent_scene.AddPhysicsActorTaint(this);

-            }

-        }

-

-        public override PhysicsVector Torque

-        {

-            get

-            {

-                if (!m_isphysical || Body.Handle == IntPtr.Zero)

-                    return new PhysicsVector(0, 0, 0);

-

-                return _torque;

-            }

-

-            set

-            {

-                m_taintTorque = value;

-                _parent_scene.AddPhysicsActorTaint(this);

-            }

-        }

-

-        public override float CollisionScore

-        {

-            get { return m_collisionscore; }

-            set { m_collisionscore = value; }

-        }

-

-        public override PhysicsVector Acceleration

-        {

-            get { return _acceleration; }

-        }

-

-        public override Quaternion Orientation

-        {

-            get { return _orientation; }

-            set { _orientation = value; }

-        }

-

-        public override int PhysicsActorType

-        {

-            get { return (int)ActorTypes.Prim; }

-            set { return; }

-        }

-

-        public override bool IsPhysical

-        {

-            get { return m_isphysical; }

-            set { m_isphysical = value; }

-        }

-

-        public override bool Flying

-        {

-            // no flying prims for you

-            get { return false; }

-            set { }

-        }

-

-        public override bool SetAlwaysRun

-        {

-            get { return false; }

-            set { return; }

-        }

-

-        public override bool ThrottleUpdates

-        {

-            get { return m_throttleUpdates; }

-            set { m_throttleUpdates = value; }

-        }

-

-        public override bool IsColliding

-        {

-            get { return iscolliding; }

-            set { iscolliding = value; }

-        }

-

-        public override bool CollidingGround

-        {

-            get { return false; }

-            set { return; }

-        }

-

-        public override bool CollidingObj

-        {

-            get { return false; }

-            set { return; }

-        }

-

-        public override bool FloatOnWater

-        {

-            set

-            {

-                m_taintCollidesWater = value;

-                _parent_scene.AddPhysicsActorTaint(this);

-            }

-        }

-

-        public override PhysicsVector RotationalVelocity

-        {

-            get

-            {

-                PhysicsVector pv = new PhysicsVector(0, 0, 0);

-                if (_zeroFlag)

-                    return pv;

-                m_lastUpdateSent = false;

-

-                if (m_rotationalVelocity.IsIdentical(pv, 0.2f))

-                    return pv;

-

-                return m_rotationalVelocity;

-            }

-            set { m_rotationalVelocity = value; }

-        }

-

-        public override bool Kinematic

-        {

-            get { return false; }

-            set { }

-        }

-

-        public override float Buoyancy

-        {

-            get { return m_buoyancy; }

-            set { m_buoyancy = value; }

-        }

-

-        public override PhysicsVector PIDTarget { set { m_PIDTarget = value; ; } }

-        public override bool PIDActive { set { m_usePID = value; } }

-        public override float PIDTau { set { m_PIDTau = value; } }

-

-        public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }

-        public override bool PIDHoverActive { set { m_useHoverPID = value; } }

-        public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }

-        public override float PIDHoverTau { set { m_PIDHoverTau = value; } }

-

-

-        public override void AddForce(PhysicsVector force, bool pushforce)

-        {

-            m_forcelist.Add(force);

-            m_taintforce = true;

-            //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() +  " to prim at " + Position.ToString());

-        }

-

-        public override void AddAngularForce(PhysicsVector force, bool pushforce)

-        {

-            m_angularforcelist.Add(force);

-            m_taintaddangularforce = true;

-        }

-

-        public override void SetMomentum(PhysicsVector momentum)

-        {

-        }

-

-        public override void SubscribeEvents(int ms)

-        {

-            m_eventsubscription = ms;

-            _parent_scene.addCollisionEventReporting(this);

-        }

-

-        public override void UnSubscribeEvents()

-        {

-            _parent_scene.remCollisionEventReporting(this);

-            m_eventsubscription = 0;

-        }

-

-        public override bool SubscribedEvents()

-        {

-            return (m_eventsubscription > 0);

-        }

-

-        #endregion

-

-

-

-        internal void Dispose()

-        {

-            //TODO:

-            DisposeOfBody();

-            SetCollisionShape(null);

-

-            if (tempMotionState3 != null && tempMotionState3.Handle != IntPtr.Zero)

-            {

-                tempMotionState3.Dispose();

-                tempMotionState3 = null;

-            }

-

-            if (tempMotionState2 != null && tempMotionState2.Handle != IntPtr.Zero)

-            {

-                tempMotionState2.Dispose();

-                tempMotionState2 = null;

-            }

-

-            if (tempMotionState1 != null && tempMotionState1.Handle != IntPtr.Zero)

-            {

-                tempMotionState1.Dispose();

-                tempMotionState1 = null;

-            }

-

-            if (tempTransform4 != null && tempTransform4.Handle != IntPtr.Zero)

-            {

-                tempTransform4.Dispose();

-                tempTransform4 = null;

-            }

-

-            if (tempTransform3 != null && tempTransform3.Handle != IntPtr.Zero)

-            {

-                tempTransform3.Dispose();

-                tempTransform3 = null;

-            }

-

-            if (tempTransform2 != null && tempTransform2.Handle != IntPtr.Zero)

-            {

-                tempTransform2.Dispose();

-                tempTransform2 = null;

-            }

-

-            if (tempTransform1 != null && tempTransform1.Handle != IntPtr.Zero)

-            {

-                tempTransform1.Dispose();

-                tempTransform1 = null;

-            }

-

-            if (tempOrientation2 != null && tempOrientation2.Handle != IntPtr.Zero)

-            {

-                tempOrientation2.Dispose();

-                tempOrientation2 = null;

-            }

-

-            if (tempOrientation1 != null && tempOrientation1.Handle != IntPtr.Zero)

-            {

-                tempOrientation1.Dispose();

-                tempOrientation1 = null;

-            }

-

-            if (tempInertia1 != null && tempInertia1.Handle != IntPtr.Zero)

-            {

-                tempInertia1.Dispose();

-                tempInertia1 = null;

-            }

-

-            if (tempInertia2 != null && tempInertia2.Handle != IntPtr.Zero)

-            {

-                tempInertia2.Dispose();

-                tempInertia1 = null;

-            }

-            

-            

-            if (tempAngularVelocity2 != null && tempAngularVelocity2.Handle != IntPtr.Zero)

-            {

-                tempAngularVelocity2.Dispose();

-                tempAngularVelocity2 = null;

-            }

-

-            if (tempAngularVelocity1 != null && tempAngularVelocity1.Handle != IntPtr.Zero)

-            {

-                tempAngularVelocity1.Dispose();

-                tempAngularVelocity1 = null;

-            }

-

-            if (tempLinearVelocity2 != null && tempLinearVelocity2.Handle != IntPtr.Zero)

-            {

-                tempLinearVelocity2.Dispose();

-                tempLinearVelocity2 = null;

-            }

-

-            if (tempLinearVelocity1 != null && tempLinearVelocity1.Handle != IntPtr.Zero)

-            {

-                tempLinearVelocity1.Dispose();

-                tempLinearVelocity1 = null;

-            }

-

-            if (tempSize2 != null && tempSize2.Handle != IntPtr.Zero)

-            {

-                tempSize2.Dispose();

-                tempSize2 = null;

-            }

-

-            if (tempSize1 != null && tempSize1.Handle != IntPtr.Zero)

-            {

-                tempSize1.Dispose();

-                tempSize1 = null;

-            }

-

-            if (tempPosition3 != null && tempPosition3.Handle != IntPtr.Zero)

-            {

-                tempPosition3.Dispose();

-                tempPosition3 = null;

-            }

-

-            if (tempPosition2 != null && tempPosition2.Handle != IntPtr.Zero)

-            {

-                tempPosition2.Dispose();

-                tempPosition2 = null;

-            }

-

-            if (tempPosition1 != null && tempPosition1.Handle != IntPtr.Zero)

-            {

-                tempPosition1.Dispose();

-                tempPosition1 = null;

-            }

-

-        }

-

-

-

-        public void ProcessTaints(float timestep)

-        {

-            if (m_taintadd)

-            {

-                changeadd(timestep);

-            }

-

-            if (prim_geom.Handle == IntPtr.Zero)

-            {

-                CreateGeom(IntPtr.Zero, primMesh);

-                

-                if (IsPhysical)

-                    SetBody(Mass);

-                else

-                    SetBody(0);

-

-            }

-            

-            if (!_position.IsIdentical(m_taintposition, 0f))

-                changemove(timestep);

-

-            if (m_taintrot != _orientation)

-                rotate(timestep);

-            //

-

-            if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent))

-                changePhysicsStatus(timestep);

-            //

-

-            if (!_size.IsIdentical(m_taintsize, 0))

-                changesize(timestep);

-            //

-

-            if (m_taintshape)

-                changeshape(timestep);

-            //

-

-            if (m_taintforce)

-                changeAddForce(timestep);

-

-            if (m_taintaddangularforce)

-                changeAddAngularForce(timestep);

-

-            if (!m_taintTorque.IsIdentical(PhysicsVector.Zero, 0.001f))

-                changeSetTorque(timestep);

-

-            if (m_taintdisable)

-                changedisable(timestep);

-

-            if (m_taintselected != m_isSelected)

-                changeSelectedStatus(timestep);

-

-            if (!m_taintVelocity.IsIdentical(PhysicsVector.Zero, 0.001f))

-                changevelocity(timestep);

-

-            if (m_taintparent != _parent)

-                changelink(timestep);

-

-            if (m_taintCollidesWater != m_collidesWater)

-                changefloatonwater(timestep);

-

-            if (!m_angularlock.IsIdentical(m_taintAngularLock, 0))

-                changeAngularLock(timestep);

-            

-

-        }

-

-        #region Physics Scene Change Action routines

-

-        private void changeadd(float timestep)

-        {

-            if (_mesh == null)

-            {

-                if (_parent_scene.needsMeshing(_pbs))

-                {

-                    // Don't need to re-enable body..   it's done in SetMesh

-                    _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical);

-                    // createmesh returns null when it's a shape that isn't a cube.

-                }

-            }

-            CreateGeom(IntPtr.Zero, primMesh);

-

-            enableBody();

-            changeSelectedStatus(timestep);

-            m_taintadd = false;

-

-        }

-

-        private void changemove(float timestep)

-        {

-

-

-            tempTransform2 = Body.getWorldTransform();

-            btQuaternion quat = tempTransform2.getRotation();

-            tempPosition2.setValue(_position.X, _position.Y, _position.Z);

-            tempTransform2.Dispose();

-            tempTransform2 = new btTransform(quat, tempPosition2);

-            Body.setWorldTransform(tempTransform2);

-

-            changeSelectedStatus(timestep);

-

-            resetCollisionAccounting();

-            m_taintposition = _position;

-        }

-

-        private void rotate(float timestep)

-        {

-

-            tempTransform2 = Body.getWorldTransform();

-            tempOrientation2 = new btQuaternion(_orientation.X, _orientation.Y, _orientation.Z, _orientation.W);

-            tempTransform2.setRotation(tempOrientation2);

-            Body.setWorldTransform(tempTransform2);

-

-            resetCollisionAccounting();

-            m_taintrot = _orientation;

-        }

-

-        private void changePhysicsStatus(float timestep)

-        {

-            SetCollisionShape(prim_geom);

-            SetBody(Mass);

-            changeSelectedStatus(timestep);

-

-            resetCollisionAccounting();

-            m_taintPhysics = m_isphysical;

-        }

-

-        private void changesize(float timestep)

-        {

-            SetCollisionShape(null);

-            // Construction of new prim

-            if (_parent_scene.needsMeshing(_pbs))

-            {

-                float meshlod = _parent_scene.meshSculptLOD;

-

-                if (IsPhysical)

-                    meshlod = _parent_scene.MeshSculptphysicalLOD;

-                // Don't need to re-enable body..   it's done in SetMesh

-

-                IMesh mesh = null;

-

-                if (_parent_scene.needsMeshing(_pbs))

-                    mesh = _parent_scene.mesher.CreateMesh(SOPName, _pbs, _size, meshlod, IsPhysical);

-

-                //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);

-

-                CreateGeom(IntPtr.Zero, mesh);

-

-

-            }

-            else

-            {

-                _mesh = null;

-                CreateGeom(IntPtr.Zero, _mesh);

-            }

-            

-            if (IsPhysical)

-                SetBody(Mass);

-            else

-                SetBody(0);

-

-            m_taintsize = _size;

-

-        }

-

-        private void changeshape(float timestep)

-        {

-            // Cleanup of old prim geometry and Bodies

-            if (IsPhysical && Body != null && Body.Handle != IntPtr.Zero)

-            {

-                if (childPrim)

-                {

-                    if (_parent != null)

-                    {

-                        BulletDotNETPrim parent = (BulletDotNETPrim)_parent;

-                        parent.ChildDelink(this);

-                    }

-                }

-                else

-                {

-                    disableBody();

-                }

-            }

-            try

-            {

-                SetCollisionShape(null);

-            }

-            catch (System.AccessViolationException)

-            {

-                //prim_geom = IntPtr.Zero;

-                m_log.Error("[PHYSICS]: PrimGeom dead");

-            }

-            

-            // we don't need to do space calculation because the client sends a position update also.

-            if (_size.X <= 0) _size.X = 0.01f;

-            if (_size.Y <= 0) _size.Y = 0.01f;

-            if (_size.Z <= 0) _size.Z = 0.01f;

-            // Construction of new prim

-

-            if (_parent_scene.needsMeshing(_pbs))

-            {

-                // Don't need to re-enable body..   it's done in SetMesh

-                float meshlod = _parent_scene.meshSculptLOD;

-

-                if (IsPhysical)

-                    meshlod = _parent_scene.MeshSculptphysicalLOD;

-

-                IMesh mesh = _parent_scene.mesher.CreateMesh(SOPName, _pbs, _size, meshlod, IsPhysical);

-                // createmesh returns null when it doesn't mesh.

-                CreateGeom(IntPtr.Zero, mesh);

-            }

-            else

-            {

-                _mesh = null;

-                CreateGeom(IntPtr.Zero, null);

-            }

-            tempPosition1.setValue(_position.X, _position.Y, _position.Z);

-            if (tempOrientation1.Handle != IntPtr.Zero)

-                tempOrientation1.Dispose();

-            tempOrientation1 = new btQuaternion(_orientation.X, Orientation.Y, _orientation.Z, _orientation.W);

-            if (tempTransform1 != null && tempTransform1.Handle != IntPtr.Zero)

-                tempTransform1.Dispose();

-            tempTransform1 = new btTransform(tempOrientation1, tempPosition1);

-

-            

-            

-

-            //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);

-            if (IsPhysical)

-            {

-                SetBody(Mass);

-                // Re creates body on size.

-                // EnableBody also does setMass()

-               

-            }

-            else

-            {

-                SetBody(0);

-            }

-

-            changeSelectedStatus(timestep);

-            if (childPrim)

-            {

-                if (_parent is BulletDotNETPrim)

-                {

-                    BulletDotNETPrim parent = (BulletDotNETPrim)_parent;

-                    parent.ChildSetGeom(this);

-                }

-            }

-            resetCollisionAccounting();

-            m_taintshape = false;

-        }

-

-        private void resetCollisionAccounting()

-        {

-            m_collisionscore = 0;

-        }

-

-        private void ChildSetGeom(BulletDotNETPrim bulletDotNETPrim)

-        {

-            // TODO: throw new NotImplementedException();

-        }

-

-        private void changeAddForce(float timestep)

-        {

-            // TODO: throw new NotImplementedException();

-        }

-

-        private void changeAddAngularForce(float timestep)

-        {

-            // TODO: throw new NotImplementedException();

-        }

-

-        private void changeSetTorque(float timestep)

-        {

-            // TODO: throw new NotImplementedException();

-        }

-

-        private void changedisable(float timestep)

-        {

-            // TODO: throw new NotImplementedException();

-        }

-

-        private void changeSelectedStatus(float timestep)

-        {

-            // TODO: throw new NotImplementedException();

-        }

-

-        private void changevelocity(float timestep)

-        {

-            // TODO: throw new NotImplementedException();

-        }

-

-        private void changelink(float timestep)

-        {

-            // TODO: throw new NotImplementedException();

-        }

-

-        private void changefloatonwater(float timestep)

-        {

-            // TODO: throw new NotImplementedException();

-        }

-

-        private void changeAngularLock(float timestep)

-        {

-            // TODO: throw new NotImplementedException();

-        }

-        #endregion

-

-

-

-

-        internal void Move(float timestep)

-        {

-            //TODO:

-            float fx = 0;

-            float fy = 0;

-            float fz = 0;

-

-            if (IsPhysical && Body != null && Body.Handle != IntPtr.Zero && !m_isSelected)

-            {

-                float m_mass = CalculateMass();

-

-                fz = 0f;

-                //m_log.Info(m_collisionFlags.ToString());

-

-                if (m_buoyancy != 0)

-                {

-                    if (m_buoyancy > 0)

-                    {

-                        fz = (((-1 * _parent_scene.gravityz) * m_buoyancy) * m_mass);

-

-                        //d.Vector3 l_velocity = d.BodyGetLinearVel(Body);

-                        //m_log.Info("Using Buoyancy: " + buoyancy + " G: " + (_parent_scene.gravityz * m_buoyancy) + "mass:" + m_mass + "  Pos: " + Position.ToString());

-                    }

-                    else

-                    {

-                        fz = (-1 * (((-1 * _parent_scene.gravityz) * (-1 * m_buoyancy)) * m_mass));

-                    }

-                }

-

-                if (m_usePID)

-                {

-                    //if (!d.BodyIsEnabled(Body))

-                    //d.BodySetForce(Body, 0f, 0f, 0f);

-                    // If we're using the PID controller, then we have no gravity

-                    fz = (-1 * _parent_scene.gravityz) * m_mass;

-

-                    //  no lock; for now it's only called from within Simulate()

-

-                    // If the PID Controller isn't active then we set our force

-                    // calculating base velocity to the current position

-

-                    if ((m_PIDTau < 1) && (m_PIDTau != 0))

-                    {

-                        //PID_G = PID_G / m_PIDTau;

-                        m_PIDTau = 1;

-                    }

-

-                    if ((PID_G - m_PIDTau) <= 0)

-                    {

-                        PID_G = m_PIDTau + 1;

-                    }

-

-                    // TODO: NEED btVector3 for Linear Velocity

-                    // NEED btVector3 for Position

-

-                    PhysicsVector pos = new PhysicsVector(0, 0, 0); //TODO: Insert values gotten from bullet

-                    PhysicsVector vel = new PhysicsVector(0, 0, 0);

-

-                    _target_velocity =

-                        new PhysicsVector(

-                            (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),

-                            (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),

-                            (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)

-                            );

-

-                    if (_target_velocity.IsIdentical(PhysicsVector.Zero, 0.1f))

-                    {

-

-                        /* TODO: Do Bullet equiv

-                         * 

-                        d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);

-                        d.BodySetLinearVel(Body, 0, 0, 0);

-                        d.BodyAddForce(Body, 0, 0, fz);

-                        return;

-                        */

-                    }

-                    else

-                    {

-                        _zeroFlag = false;

-

-                        fx = ((_target_velocity.X) - vel.X) * (PID_D);

-                        fy = ((_target_velocity.Y) - vel.Y) * (PID_D);

-                        fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);

-

-                    }

-

-                }

-

-                if (m_useHoverPID && !m_usePID)

-                {

-                    // If we're using the PID controller, then we have no gravity

-                    fz = (-1 * _parent_scene.gravityz) * m_mass;

-

-                    //  no lock; for now it's only called from within Simulate()

-

-                    // If the PID Controller isn't active then we set our force

-                    // calculating base velocity to the current position

-

-                    if ((m_PIDTau < 1))

-                    {

-                        PID_G = PID_G / m_PIDTau;

-                    }

-

-                    if ((PID_G - m_PIDTau) <= 0)

-                    {

-                        PID_G = m_PIDTau + 1;

-                    }

-                    PhysicsVector pos = new PhysicsVector(0, 0, 0); //TODO: Insert values gotten from bullet

-                    PhysicsVector vel = new PhysicsVector(0, 0, 0);

-

-                    // determine what our target height really is based on HoverType

-                    switch (m_PIDHoverType)

-                    {

-                        case PIDHoverType.Absolute:

-                            m_targetHoverHeight = m_PIDHoverHeight;

-                            break;

-                        case PIDHoverType.Ground:

-                            m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);

-                            m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;

-                            break;

-                        case PIDHoverType.GroundAndWater:

-                            m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);

-                            m_waterHeight = _parent_scene.GetWaterLevel();

-                            if (m_groundHeight > m_waterHeight)

-                            {

-                                m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;

-                            }

-                            else

-                            {

-                                m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;

-                            }

-                            break;

-                        case PIDHoverType.Water:

-                            m_waterHeight = _parent_scene.GetWaterLevel();

-                            m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;

-                            break;

-                    }

-

-

-                    _target_velocity =

-                        new PhysicsVector(0.0f, 0.0f,

-                            (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)

-                            );

-

-                    //  if velocity is zero, use position control; otherwise, velocity control

-

-                    if (_target_velocity.IsIdentical(PhysicsVector.Zero, 0.1f))

-                    {

-

-                        /* TODO: Do Bullet Equiv

-                        d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);

-                        d.BodySetLinearVel(Body, vel.X, vel.Y, 0);

-                        d.BodyAddForce(Body, 0, 0, fz);

-                        */

-                        return;

-                    }

-                    else

-                    {

-                        _zeroFlag = false;

-

-                        // We're flying and colliding with something

-                        fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);

-                    }

-                }

-

-                fx *= m_mass;

-                fy *= m_mass;

-                //fz *= m_mass;

-

-                fx += m_force.X;

-                fy += m_force.Y;

-                fz += m_force.Z;

-

-                //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());

-                if (fx != 0 || fy != 0 || fz != 0)

-                {

-                    /*

-                     * TODO: Do Bullet Equiv

-                    if (!d.BodyIsEnabled(Body))

-                    {

-                        d.BodySetLinearVel(Body, 0f, 0f, 0f);

-                        d.BodySetForce(Body, 0, 0, 0);

-                        enableBodySoft();

-                    }

-                    */

-                    // 35x10 = 350n times the mass per second applied maximum.

-

-                    float nmax = 35f * m_mass;

-                    float nmin = -35f * m_mass;

-

-

-                    if (fx > nmax)

-                        fx = nmax;

-                    if (fx < nmin)

-                        fx = nmin;

-                    if (fy > nmax)

-                        fy = nmax;

-                    if (fy < nmin)

-                        fy = nmin;

-

-                    // TODO: Do Bullet Equiv

-                    // d.BodyAddForce(Body, fx, fy, fz);

-                }

-            }

-            else

-            {

-                // _zeroPosition = d.BodyGetPosition(Body);

-                return;

-            }

-        }

-

-

-

-

-        #region Mass Calculation

-

-        private float CalculateMass()

-        {

-            float volume = 0;

-

-            // No material is passed to the physics engines yet..  soo..

-            // we're using the m_density constant in the class definition

-

-            float returnMass = 0;

-

-            switch (_pbs.ProfileShape)

-            {

-                case ProfileShape.Square:

-                    // Profile Volume

-

-                    volume = _size.X * _size.Y * _size.Z;

-

-                    // If the user has 'hollowed out'

-                    // ProfileHollow is one of those 0 to 50000 values :P

-                    // we like percentages better..   so turning into a percentage

-

-                    if (((float)_pbs.ProfileHollow / 50000f) > 0.0)

-                    {

-                        float hollowAmount = (float)_pbs.ProfileHollow / 50000f;

-

-                        // calculate the hollow volume by it's shape compared to the prim shape

-                        float hollowVolume = 0;

-                        switch (_pbs.HollowShape)

-                        {

-                            case HollowShape.Square:

-                            case HollowShape.Same:

-                                // Cube Hollow volume calculation

-                                float hollowsizex = _size.X * hollowAmount;

-                                float hollowsizey = _size.Y * hollowAmount;

-                                float hollowsizez = _size.Z * hollowAmount;

-                                hollowVolume = hollowsizex * hollowsizey * hollowsizez;

-                                break;

-

-                            case HollowShape.Circle:

-                                // Hollow shape is a perfect cyllinder in respect to the cube's scale

-                                // Cyllinder hollow volume calculation

-                                float hRadius = _size.X / 2;

-                                float hLength = _size.Z;

-

-                                // pi * r2 * h

-                                hollowVolume = ((float)(Math.PI * Math.Pow(hRadius, 2) * hLength) * hollowAmount);

-                                break;

-

-                            case HollowShape.Triangle:

-                                // Equilateral Triangular Prism volume hollow calculation

-                                // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y

-

-                                float aLength = _size.Y;

-                                // 1/2 abh

-                                hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount);

-                                break;

-

-                            default:

-                                hollowVolume = 0;

-                                break;

-                        }

-                        volume = volume - hollowVolume;

-                    }

-

-                    break;

-                case ProfileShape.Circle:

-                    if (_pbs.PathCurve == (byte)Extrusion.Straight)

-                    {

-                        // Cylinder

-                        float volume1 = (float)(Math.PI * Math.Pow(_size.X / 2, 2) * _size.Z);

-                        float volume2 = (float)(Math.PI * Math.Pow(_size.Y / 2, 2) * _size.Z);

-

-                        // Approximating the cylinder's irregularity.

-                        if (volume1 > volume2)

-                        {

-                            volume = (float)volume1 - (volume1 - volume2);

-                        }

-                        else if (volume2 > volume1)

-                        {

-                            volume = (float)volume2 - (volume2 - volume1);

-                        }

-                        else

-                        {

-                            // Regular cylinder

-                            volume = volume1;

-                        }

-                    }

-                    else

-                    {

-                        // We don't know what the shape is yet, so use default

-                        volume = _size.X * _size.Y * _size.Z;

-                    }

-                    // If the user has 'hollowed out'

-                    // ProfileHollow is one of those 0 to 50000 values :P

-                    // we like percentages better..   so turning into a percentage

-

-                    if (((float)_pbs.ProfileHollow / 50000f) > 0.0)

-                    {

-                        float hollowAmount = (float)_pbs.ProfileHollow / 50000f;

-

-                        // calculate the hollow volume by it's shape compared to the prim shape

-                        float hollowVolume = 0;

-                        switch (_pbs.HollowShape)

-                        {

-                            case HollowShape.Same:

-                            case HollowShape.Circle:

-                                // Hollow shape is a perfect cyllinder in respect to the cube's scale

-                                // Cyllinder hollow volume calculation

-                                float hRadius = _size.X / 2;

-                                float hLength = _size.Z;

-

-                                // pi * r2 * h

-                                hollowVolume = ((float)(Math.PI * Math.Pow(hRadius, 2) * hLength) * hollowAmount);

-                                break;

-

-                            case HollowShape.Square:

-                                // Cube Hollow volume calculation

-                                float hollowsizex = _size.X * hollowAmount;

-                                float hollowsizey = _size.Y * hollowAmount;

-                                float hollowsizez = _size.Z * hollowAmount;

-                                hollowVolume = hollowsizex * hollowsizey * hollowsizez;

-                                break;

-

-                            case HollowShape.Triangle:

-                                // Equilateral Triangular Prism volume hollow calculation

-                                // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y

-

-                                float aLength = _size.Y;

-                                // 1/2 abh

-                                hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount);

-                                break;

-

-                            default:

-                                hollowVolume = 0;

-                                break;

-                        }

-                        volume = volume - hollowVolume;

-                    }

-                    break;

-

-                case ProfileShape.HalfCircle:

-                    if (_pbs.PathCurve == (byte)Extrusion.Curve1)

-                    {

-                        if (_size.X == _size.Y && _size.Z == _size.X)

-                        {

-                            // regular sphere

-                            // v = 4/3 * pi * r^3

-                            float sradius3 = (float)Math.Pow((_size.X / 2), 3);

-                            volume = (float)((4 / 3f) * Math.PI * sradius3);

-                        }

-                        else

-                        {

-                            // we treat this as a box currently

-                            volume = _size.X * _size.Y * _size.Z;

-                        }

-                    }

-                    else

-                    {

-                        // We don't know what the shape is yet, so use default

-                        volume = _size.X * _size.Y * _size.Z;

-                    }

-                    break;

-

-                case ProfileShape.EquilateralTriangle:

-                    /*

-                        v = (abs((xB*yA-xA*yB)+(xC*yB-xB*yC)+(xA*yC-xC*yA))/2) * h

-

-                        // seed mesh

-                        Vertex MM = new Vertex(-0.25f, -0.45f, 0.0f);

-                        Vertex PM = new Vertex(+0.5f, 0f, 0.0f);

-                        Vertex PP = new Vertex(-0.25f, +0.45f, 0.0f);

-                     */

-                    float xA = -0.25f * _size.X;

-                    float yA = -0.45f * _size.Y;

-

-                    float xB = 0.5f * _size.X;

-                    float yB = 0;

-

-                    float xC = -0.25f * _size.X;

-                    float yC = 0.45f * _size.Y;

-

-                    volume = (float)((Math.Abs((xB * yA - xA * yB) + (xC * yB - xB * yC) + (xA * yC - xC * yA)) / 2) * _size.Z);

-

-                    // If the user has 'hollowed out'

-                    // ProfileHollow is one of those 0 to 50000 values :P

-                    // we like percentages better..   so turning into a percentage

-                    float fhollowFactor = ((float)_pbs.ProfileHollow / 1.9f);

-                    if (((float)fhollowFactor / 50000f) > 0.0)

-                    {

-                        float hollowAmount = (float)fhollowFactor / 50000f;

-

-                        // calculate the hollow volume by it's shape compared to the prim shape

-                        float hollowVolume = 0;

-                        switch (_pbs.HollowShape)

-                        {

-                            case HollowShape.Same:

-                            case HollowShape.Triangle:

-                                // Equilateral Triangular Prism volume hollow calculation

-                                // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y

-

-                                float aLength = _size.Y;

-                                // 1/2 abh

-                                hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount);

-                                break;

-

-                            case HollowShape.Square:

-                                // Cube Hollow volume calculation

-                                float hollowsizex = _size.X * hollowAmount;

-                                float hollowsizey = _size.Y * hollowAmount;

-                                float hollowsizez = _size.Z * hollowAmount;

-                                hollowVolume = hollowsizex * hollowsizey * hollowsizez;

-                                break;

-

-                            case HollowShape.Circle:

-                                // Hollow shape is a perfect cyllinder in respect to the cube's scale

-                                // Cyllinder hollow volume calculation

-                                float hRadius = _size.X / 2;

-                                float hLength = _size.Z;

-

-                                // pi * r2 * h

-                                hollowVolume = ((float)((Math.PI * Math.Pow(hRadius, 2) * hLength) / 2) * hollowAmount);

-                                break;

-

-                            default:

-                                hollowVolume = 0;

-                                break;

-                        }

-                        volume = volume - hollowVolume;

-                    }

-                    break;

-

-                default:

-                    // we don't have all of the volume formulas yet so

-                    // use the common volume formula for all

-                    volume = _size.X * _size.Y * _size.Z;

-                    break;

-            }

-

-            // Calculate Path cut effect on volume

-            // Not exact, in the triangle hollow example

-            // They should never be zero or less then zero..

-            // we'll ignore it if it's less then zero

-

-            // ProfileEnd and ProfileBegin are values

-            // from 0 to 50000

-

-            // Turning them back into percentages so that I can cut that percentage off the volume

-

-            float PathCutEndAmount = _pbs.ProfileEnd;

-            float PathCutStartAmount = _pbs.ProfileBegin;

-            if (((PathCutStartAmount + PathCutEndAmount) / 50000f) > 0.0f)

-            {

-                float pathCutAmount = ((PathCutStartAmount + PathCutEndAmount) / 50000f);

-

-                // Check the return amount for sanity

-                if (pathCutAmount >= 0.99f)

-                    pathCutAmount = 0.99f;

-

-                volume = volume - (volume * pathCutAmount);

-            }

-            UInt16 taperX = _pbs.PathScaleX;

-            UInt16 taperY = _pbs.PathScaleY;

-            float taperFactorX = 0;

-            float taperFactorY = 0;

-

-            // Mass = density * volume

-            if (taperX != 100)

-            {

-                if (taperX > 100)

-                {

-                    taperFactorX = 1.0f - ((float)taperX / 200);

-                    //m_log.Warn("taperTopFactorX: " + extr.taperTopFactorX.ToString());

-                }

-                else

-                {

-                    taperFactorX = 1.0f - ((100 - (float)taperX) / 100);

-                    //m_log.Warn("taperBotFactorX: " + extr.taperBotFactorX.ToString());

-                }

-                volume = (float)volume * ((taperFactorX / 3f) + 0.001f);

-            }

-

-            if (taperY != 100)

-            {

-                if (taperY > 100)

-                {

-                    taperFactorY = 1.0f - ((float)taperY / 200);

-                    //m_log.Warn("taperTopFactorY: " + extr.taperTopFactorY.ToString());

-                }

-                else

-                {

-                    taperFactorY = 1.0f - ((100 - (float)taperY) / 100);

-                    //m_log.Warn("taperBotFactorY: " + extr.taperBotFactorY.ToString());

-                }

-                volume = (float)volume * ((taperFactorY / 3f) + 0.001f);

-            }

-            returnMass = m_density * volume;

-            if (returnMass <= 0) returnMass = 0.0001f;//ckrinke: Mass must be greater then zero.

-

-

-

-            // Recursively calculate mass

-            bool HasChildPrim = false;

-            lock (childrenPrim)

-            {

-                if (childrenPrim.Count > 0)

-                {

-                    HasChildPrim = true;

-                }

-

-            }

-            if (HasChildPrim)

-            {

-                BulletDotNETPrim[] childPrimArr = new BulletDotNETPrim[0];

-

-                lock (childrenPrim)

-                    childPrimArr = childrenPrim.ToArray();

-

-                for (int i = 0; i < childPrimArr.Length; i++)

-                {

-                    if (childPrimArr[i] != null && !childPrimArr[i].m_taintremove)

-                        returnMass += childPrimArr[i].CalculateMass();

-                    // failsafe, this shouldn't happen but with OpenSim, you never know :)

-                    if (i > 256)

-                        break;

-                }

-            }

-

-

-

-

-

-            return returnMass;

-        }

-

-        #endregion

-

-

-        public void CreateGeom(IntPtr m_targetSpace, IMesh p_mesh)

-        {

-            if (p_mesh != null)

-            {

-                _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical);

-                setMesh(_parent_scene, _mesh);

-                

-            }

-            else

-            {

-                if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1)

-                {

-                    if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z)

-                    {

-                        if (((_size.X / 2f) > 0f))

-                        {

-                            //SetGeom to a Regular Sphere

-                            tempSize1.setValue(_size.X * 0.5f, _size.Y * 0.5f, _size.Z * 0.5f);

-                            SetCollisionShape(new btSphereShape(_size.X*0.5f));

-                        }

-                        else

-                        {

-                            // uses halfextents

-                            tempSize1.setValue(_size.X*0.5f, _size.Y*0.5f, _size.Z*0.5f);

-                            SetCollisionShape(new btBoxShape(tempSize1));

-                        }

-                    }

-                    else

-                    {

-                        // uses halfextents

-                        tempSize1.setValue(_size.X * 0.5f, _size.Y * 0.5f, _size.Z * 0.5f);

-                        SetCollisionShape(new btBoxShape(tempSize1));

-                    }

-

-                }

-                else

-                {

-                    // uses halfextents

-                    tempSize1.setValue(_size.X * 0.5f, _size.Y * 0.5f, _size.Z * 0.5f);

-                    SetCollisionShape(new btBoxShape(tempSize1));

-                }

-            }

-        }

-

-        private void setMesh(BulletDotNETScene _parent_scene, IMesh mesh)

-        {

-            // TODO: Set Collision Body Mesh

-            // This sleeper is there to moderate how long it takes between

-            // setting up the mesh and pre-processing it when we get rapid fire mesh requests on a single object

-

-            Thread.Sleep(10);

-

-            //Kill Body so that mesh can re-make the geom

-            if (IsPhysical && Body != null && Body.Handle != IntPtr.Zero)

-            {

-                if (childPrim)

-                {

-                    if (_parent != null)

-                    {

-                        BulletDotNETPrim parent = (BulletDotNETPrim)_parent;

-                        parent.ChildDelink(this);

-                    }

-                }

-                else

-                {

-                    disableBody();

-                }

-            }

-

-            IMesh oldMesh = primMesh;

-

-            primMesh = mesh;

-            

-            float[] vertexList = primMesh.getVertexListAsFloatLocked(); // Note, that vertextList is pinned in memory

-            int[] indexList = primMesh.getIndexListAsIntLocked(); // Also pinned, needs release after usage

-            //Array.Reverse(indexList);

-            primMesh.releaseSourceMeshData(); // free up the original mesh data to save memory

-

-            int VertexCount = vertexList.GetLength(0) / 3;

-            int IndexCount = indexList.GetLength(0);

-

-            if (btshapeArray != null && btshapeArray.Handle != IntPtr.Zero)

-                btshapeArray.Dispose();

-            //Array.Reverse(indexList);

-            btshapeArray = new btTriangleIndexVertexArray(IndexCount / 3, indexList, (3 * sizeof(int)),

-                                                                                     VertexCount, vertexList, 3*sizeof (float));

-            SetCollisionShape(new btGImpactMeshShape(btshapeArray));

-            //((btGImpactMeshShape) prim_geom).updateBound();

-            ((btGImpactMeshShape)prim_geom).setLocalScaling(new btVector3(1,1, 1));

-            ((btGImpactMeshShape)prim_geom).updateBound();

-            _parent_scene.SetUsingGImpact();

-            if (oldMesh != null)

-            {

-                oldMesh.releasePinned();

-                oldMesh = null;

-            }

-

-        }

-

-        private void SetCollisionShape(btCollisionShape shape)

-        {

-            if (shape == null)

-                m_log.Debug("[PHYSICS]:SetShape!Null");

-            else

-                m_log.Debug("[PHYSICS]:SetShape!");

-

-            if (Body != null)

-            {

-                DisposeOfBody();

-            }

-

-            if (prim_geom != null)

-            {

-                prim_geom.Dispose();

-                prim_geom = null;

-            }

-            prim_geom = shape;

-           

-            //Body.set

-        }

-

-        public void SetBody(float mass)

-        {

-            m_log.DebugFormat("[PHYSICS]: SetBody! {0}",mass);

-            if (Body != null && Body.Handle != IntPtr.Zero)

-            {

-                DisposeOfBody();

-            }

-

-            if (tempMotionState1 != null && tempMotionState1.Handle != IntPtr.Zero)

-                tempMotionState1.Dispose();

-            if (tempTransform2 != null && tempTransform2.Handle != IntPtr.Zero)

-                tempTransform2.Dispose();

-            if (tempOrientation2 != null && tempOrientation2.Handle != IntPtr.Zero)

-                tempOrientation2.Dispose();

-

-            if (tempPosition2 != null && tempPosition2.Handle != IntPtr.Zero)

-                tempPosition2.Dispose();

-

-            tempOrientation2 = new btQuaternion(_orientation.X, _orientation.Y, _orientation.Z, _orientation.W);

-            tempPosition2 = new btVector3(_position.X, _position.Y, _position.Z);

-            tempTransform2 = new btTransform(tempOrientation2, tempPosition2);

-            tempMotionState1 = new btDefaultMotionState(tempTransform2, _parent_scene.TransZero);

-            if (tempInertia1 != null && tempInertia1.Handle != IntPtr.Zero)

-                tempInertia1.Dispose();

-            tempInertia1 = new btVector3(0, 0, 0);

-            if (prim_geom.Handle == IntPtr.Zero)

-            {

-                m_log.Warn("[PHYSICS]:PrimGeom is Disposed!");

-                CreateGeom(IntPtr.Zero, primMesh);

-

-            }

-            prim_geom.calculateLocalInertia(mass, tempInertia1);

-

-            if (mass == 0)

-                Body = new btRigidBody(mass, tempMotionState1, prim_geom);

-            else

-                Body = new btRigidBody(mass, tempMotionState1, prim_geom, tempInertia1);

-

-            if (prim_geom is btGImpactMeshShape)

-            {

-                ((btGImpactMeshShape) prim_geom).setLocalScaling(new btVector3(1, 1, 1));

-                ((btGImpactMeshShape) prim_geom).updateBound();

-            }

-            _parent_scene.AddPrimToScene(this);

-        }

-

-        private void DisposeOfBody()

-        {

-            if (Body != null)

-            {

-                if (Body.Handle != IntPtr.Zero)

-                {

-                    _parent_scene.removeFromWorld(this,Body);

-                    Body.Dispose();

-                }

-                Body = null;

-                // TODO: dispose parts that make up body

-            }

-        }

-

-        private void ChildDelink(BulletDotNETPrim pPrim)

-        {

-            // Okay, we have a delinked child..   need to rebuild the body.

-            lock (childrenPrim)

-            {

-                foreach (BulletDotNETPrim prm in childrenPrim)

-                {

-                    prm.childPrim = true;

-                    prm.disableBody();

-

-                }

-            }

-            disableBody();

-

-            lock (childrenPrim)

-            {

-                childrenPrim.Remove(pPrim);

-            }

-

-

-

-

-            if (Body != null && Body.Handle != IntPtr.Zero)

-            {

-                _parent_scene.remActivePrim(this);

-            }

-

-

-

-            lock (childrenPrim)

-            {

-                foreach (BulletDotNETPrim prm in childrenPrim)

-                {

-                    ParentPrim(prm);

-                }

-            }

-

-        }

-

-        private void ParentPrim(BulletDotNETPrim prm)

-        {

-            // TODO: Parent Linking algorithm.   Use btComplexObject

-        }

-

-        public void disableBody()

-        {

-            //this kills the body so things like 'mesh' can re-create it.

-            lock (this)

-            {

-                if (!childPrim)

-                {

-                    if (Body != null && Body.Handle != IntPtr.Zero)

-                    {

-                        _parent_scene.remActivePrim(this);

-

-                        m_collisionCategories &= ~CollisionCategories.Body;

-                        m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);

-

-                        if (prim_geom != null && prim_geom.Handle != IntPtr.Zero)

-                        {

-                            // TODO: Set Category bits and Flags

-                        }

-

-                        // TODO: destroy body

-                        DisposeOfBody();

-

-                        lock (childrenPrim)

-                        {

-                            if (childrenPrim.Count > 0)

-                            {

-                                foreach (BulletDotNETPrim prm in childrenPrim)

-                                {

-                                    _parent_scene.remActivePrim(prm);

-                                    prm.DisposeOfBody();

-                                    prm.SetCollisionShape(null);

-                                }

-                            }

-

-                        }

-

-                        DisposeOfBody();

-                    }

-                }

-                else

-                {

-                    _parent_scene.remActivePrim(this);

-                    m_collisionCategories &= ~CollisionCategories.Body;

-                    m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);

-

-                    if (prim_geom != null && prim_geom.Handle != IntPtr.Zero)

-                    {

-                        // TODO: Set Category bits and Flags

-                    }

-

-                    DisposeOfBody();

-                }

-                

-            }

-            m_disabled = true;

-            m_collisionscore = 0;

-        }

-

-        public void disableBodySoft()

-        {

-            m_disabled = true;

-

-            if (m_isphysical && Body.Handle != IntPtr.Zero)

-            {

-                Body.clearForces();

-                Body.forceActivationState(0);

-                

-            }

-

-        }

-

-        public void enableBodySoft()

-        {

-            if (!childPrim)

-            {

-                if (m_isphysical && Body.Handle != IntPtr.Zero)

-                {

-                    Body.clearForces();

-                    Body.forceActivationState(1);

-                }

-                m_disabled = false;

-            }

-        }

-

-        public void enableBody()

-        {

-            if (!childPrim)

-            {

-                //SetCollisionShape(prim_geom);

-                if (IsPhysical)

-                    SetBody(Mass);

-                else

-                    SetBody(0);

-                

-                // TODO: Set Collision Category Bits and Flags

-                // TODO: Set Auto Disable data

-

-                m_interpenetrationcount = 0;

-                m_collisionscore = 0;

-                m_disabled = false;

-                // The body doesn't already have a finite rotation mode set here

-                if ((!m_angularlock.IsIdentical(PhysicsVector.Zero, 0)) && _parent == null)

-                {

-                    // TODO: Create Angular Motor on Axis Lock!

-                }

-                _parent_scene.addActivePrim(this);

-            }

-        }

-

-        public void UpdatePositionAndVelocity()

-        {

-            if (_parent == null)

-            {

-                PhysicsVector pv = new PhysicsVector(0, 0, 0);

-                bool lastZeroFlag = _zeroFlag;

-                if (tempPosition2 != null && tempPosition2.Handle != IntPtr.Zero)

-                    tempPosition2.Dispose();

-                if (tempTransform3 != null && tempTransform3.Handle != IntPtr.Zero)

-                    tempTransform3.Dispose();

-

-                if (tempOrientation2 != null && tempOrientation2.Handle != IntPtr.Zero)

-                    tempOrientation2.Dispose();

-

-                if (tempAngularVelocity1 != null && tempAngularVelocity1.Handle != IntPtr.Zero)

-                    tempAngularVelocity1.Dispose();

-

-                if (tempLinearVelocity1 != null && tempLinearVelocity1.Handle != IntPtr.Zero)

-                    tempLinearVelocity1.Dispose();

-

-

-

-                tempTransform3 = Body.getInterpolationWorldTransform();

-                tempPosition2 = tempTransform3.getOrigin(); // vec

-                tempOrientation2 = tempTransform3.getRotation(); // ori

-                tempAngularVelocity1 = Body.getInterpolationAngularVelocity(); //rotvel

-                tempLinearVelocity1 = Body.getInterpolationLinearVelocity(); // vel

-

-                _torque.setValues(tempAngularVelocity1.getX(), tempAngularVelocity1.getX(), tempAngularVelocity1.getZ());

-                PhysicsVector l_position = new PhysicsVector();

-                Quaternion l_orientation = new Quaternion();

-                m_lastposition = _position;

-                m_lastorientation = _orientation;

-

-                l_position.X = tempPosition2.getX();

-                l_position.Y = tempPosition2.getY();

-                l_position.Z = tempPosition2.getZ();

-                l_orientation.X = tempOrientation2.getX();

-                l_orientation.Y = tempOrientation2.getY();

-                l_orientation.Z = tempOrientation2.getZ();

-                l_orientation.W = tempOrientation2.getW();

-

-                if (l_position.X > 255.95f || l_position.X < 0f || l_position.Y > 255.95f || l_position.Y < 0f)

-                {

-                    //base.RaiseOutOfBounds(l_position);

-

-                    if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds)

-                    {

-                        _position = l_position;

-                        //_parent_scene.remActivePrim(this);

-                        if (_parent == null)

-                            base.RequestPhysicsterseUpdate();

-                        return;

-                    }

-                    else

-                    {

-                        if (_parent == null)

-                            base.RaiseOutOfBounds(l_position);

-                        return;

-                    }

-                }

-

-                if (l_position.Z < -200000f)

-                {

-                    // This is so prim that get lost underground don't fall forever and suck up

-                    //

-                    // Sim resources and memory.

-                    // Disables the prim's movement physics....

-                    // It's a hack and will generate a console message if it fails.

-

-                    //IsPhysical = false;

-                    //if (_parent == null)

-                        //base.RaiseOutOfBounds(_position);

-

-                    _acceleration.X = 0;

-                    _acceleration.Y = 0;

-                    _acceleration.Z = 0;

-

-                    _velocity.X = 0;

-                    _velocity.Y = 0;

-                    _velocity.Z = 0;

-                    m_rotationalVelocity.X = 0;

-                    m_rotationalVelocity.Y = 0;

-                    m_rotationalVelocity.Z = 0;

-

-                    if (_parent == null)

-                        base.RequestPhysicsterseUpdate();

-

-                    m_throttleUpdates = false;

-                    throttleCounter = 0;

-                    _zeroFlag = true;

-                    //outofBounds = true;

-                }

-

-                if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)

-                    && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)

-                    && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)

-                    && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01 ))

-                {

-                    _zeroFlag = true;

-                    m_throttleUpdates = false;

-                }

-                else

-                {

-                    //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString());

-                    _zeroFlag = false;

-                }

-

-                if (_zeroFlag)

-                {

-                    _velocity.X = 0.0f;

-                    _velocity.Y = 0.0f;

-                    _velocity.Z = 0.0f;

-

-                    _acceleration.X = 0;

-                    _acceleration.Y = 0;

-                    _acceleration.Z = 0;

-

-                    //_orientation.w = 0f;

-                    //_orientation.X = 0f;

-                    //_orientation.Y = 0f;

-                    //_orientation.Z = 0f;

-                    m_rotationalVelocity.X = 0;

-                    m_rotationalVelocity.Y = 0;

-                    m_rotationalVelocity.Z = 0;

-                    if (!m_lastUpdateSent)

-                    {

-                        m_throttleUpdates = false;

-                        throttleCounter = 0;

-                        m_rotationalVelocity = pv;

-

-                        if (_parent == null)

-                            base.RequestPhysicsterseUpdate();

-

-                        m_lastUpdateSent = true;

-                    }

-                }

-                else

-                {

-                    if (lastZeroFlag != _zeroFlag)

-                    {

-                        if (_parent == null)

-                            base.RequestPhysicsterseUpdate();

-                    }

-

-                    m_lastVelocity = _velocity;

-

-                    _position = l_position;

-

-                    _velocity.X = tempLinearVelocity1.getX();

-                    _velocity.Y = tempLinearVelocity1.getY();

-                    _velocity.Z = tempLinearVelocity1.getZ();

-

-                    _acceleration = ((_velocity - m_lastVelocity) / 0.1f);

-                    _acceleration = new PhysicsVector(_velocity.X - m_lastVelocity.X / 0.1f, _velocity.Y - m_lastVelocity.Y / 0.1f, _velocity.Z - m_lastVelocity.Z / 0.1f);

-                    //m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString());

-

-                    if (_velocity.IsIdentical(pv, 0.5f))

-                    {

-                        m_rotationalVelocity = pv;

-                    }

-                    else

-                    {

-                        

-                        m_rotationalVelocity.setValues(tempAngularVelocity1.getX(), tempAngularVelocity1.getY(), tempAngularVelocity1.getZ());

-                    }

-

-                    //m_log.Debug("ODE: " + m_rotationalVelocity.ToString());

-

-                    _orientation.X = l_orientation.X;

-                    _orientation.Y = l_orientation.Y;

-                    _orientation.Z = l_orientation.Z;

-                    _orientation.W = l_orientation.W;

-                    m_lastUpdateSent = false;

-                    

-                    //if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)

-                    //{

-                        if (_parent == null)

-                            base.RequestPhysicsterseUpdate();

-                   // }

-                   // else

-                   // {

-                   //     throttleCounter++;

-                    //}

-                   

-                }

-                m_lastposition = l_position;

-            }

-            else

-            {

-                // Not a body..   so Make sure the client isn't interpolating

-                _velocity.X = 0;

-                _velocity.Y = 0;

-                _velocity.Z = 0;

-

-                _acceleration.X = 0;

-                _acceleration.Y = 0;

-                _acceleration.Z = 0;

-

-                m_rotationalVelocity.X = 0;

-                m_rotationalVelocity.Y = 0;

-                m_rotationalVelocity.Z = 0;

-                _zeroFlag = true;

-            }

-        }

-

-

-        internal void setPrimForRemoval()

-        {

-            m_taintremove = true;

-        }

-    }

-}

-                    

+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSim Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+using System;
+using System.Collections.Generic;
+using System.Reflection;
+using System.Runtime.InteropServices;
+using System.Threading;
+using log4net;
+using OpenMetaverse;
+using BulletDotNET;
+using OpenSim.Framework;
+using OpenSim.Region.Physics.Manager;
+
+namespace OpenSim.Region.Physics.BulletDotNETPlugin
+{
+    public class BulletDotNETPrim : PhysicsActor
+    {
+        private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
+
+        private PhysicsVector _position;
+        private PhysicsVector _velocity;
+        private PhysicsVector _torque = new PhysicsVector(0, 0, 0);
+        private PhysicsVector m_lastVelocity = new PhysicsVector(0.0f, 0.0f, 0.0f);
+        private PhysicsVector m_lastposition = new PhysicsVector(0.0f, 0.0f, 0.0f);
+        private Quaternion m_lastorientation = new Quaternion();
+        private PhysicsVector m_rotationalVelocity;
+        private PhysicsVector _size;
+        private PhysicsVector _acceleration;
+        // private d.Vector3 _zeroPosition = new d.Vector3(0.0f, 0.0f, 0.0f);
+        private Quaternion _orientation;
+        private PhysicsVector m_taintposition;
+        private PhysicsVector m_taintsize;
+        private PhysicsVector m_taintVelocity = new PhysicsVector(0, 0, 0);
+        private PhysicsVector m_taintTorque = new PhysicsVector(0, 0, 0);
+        private Quaternion m_taintrot;
+        private PhysicsVector m_angularlock = new PhysicsVector(1f, 1f, 1f);
+        private PhysicsVector m_taintAngularLock = new PhysicsVector(1f, 1f, 1f);
+        private btGeneric6DofConstraint Amotor;
+
+        private PhysicsVector m_PIDTarget = new PhysicsVector(0, 0, 0);
+        private float m_PIDTau = 0f;
+        private float m_PIDHoverHeight = 0f;
+        private float m_PIDHoverTau = 0f;
+        private bool m_useHoverPID = false;
+        private PIDHoverType m_PIDHoverType = PIDHoverType.Ground;
+        private float m_targetHoverHeight = 0f;
+        private float m_groundHeight = 0f;
+        private float m_waterHeight = 0f;
+        private float PID_D = 35f;
+        private float PID_G = 25f;
+        private float m_tensor = 5f;
+        private int body_autodisable_frames = 20;
+        private IMesh primMesh = null;
+
+        private bool m_usePID = false;
+
+
+        private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom
+                                                        | CollisionCategories.Space
+                                                        | CollisionCategories.Body
+                                                        | CollisionCategories.Character
+                                                     );
+
+        private bool m_taintshape = false;
+        private bool m_taintPhysics = false;
+        private bool m_collidesLand = true;
+        private bool m_collidesWater = false;
+        public bool m_returnCollisions = false;
+
+        // Default we're a Geometry
+        private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);
+
+        // Default, Collide with Other Geometries, spaces and Bodies
+        private CollisionCategories m_collisionFlags = m_default_collisionFlags;
+
+        public bool m_taintremove = false;
+        public bool m_taintdisable = false;
+        public bool m_disabled = false;
+        public bool m_taintadd = false;
+        public bool m_taintselected = false;
+        public bool m_taintCollidesWater = false;
+
+        public uint m_localID = 0;
+
+        //public GCHandle gc;
+        private CollisionLocker ode;
+
+        private bool m_taintforce = false;
+        private bool m_taintaddangularforce = false;
+        private PhysicsVector m_force = new PhysicsVector(0.0f, 0.0f, 0.0f);
+        private List<PhysicsVector> m_forcelist = new List<PhysicsVector>();
+        private List<PhysicsVector> m_angularforcelist = new List<PhysicsVector>();
+
+        private IMesh _mesh;
+        private PrimitiveBaseShape _pbs;
+        private BulletDotNETScene _parent_scene;
+        public btCollisionShape prim_geom;
+        public IntPtr _triMeshData;
+
+        private PhysicsActor _parent = null;
+        private PhysicsActor m_taintparent = null;
+
+        private List<BulletDotNETPrim> childrenPrim = new List<BulletDotNETPrim>();
+
+        private bool iscolliding = false;
+        private bool m_isphysical = false;
+        private bool m_isSelected = false;
+
+        internal bool m_isVolumeDetect = false; // If true, this prim only detects collisions but doesn't collide actively
+
+        private bool m_throttleUpdates = false;
+        private int throttleCounter = 0;
+        public int m_interpenetrationcount = 0;
+        public float m_collisionscore = 0;
+        public int m_roundsUnderMotionThreshold = 0;
+        private int m_crossingfailures = 0;
+
+        public float m_buoyancy = 0f;
+
+        public bool outofBounds = false;
+        private float m_density = 10.000006836f; // Aluminum g/cm3;
+
+        public bool _zeroFlag = false;
+        private bool m_lastUpdateSent = false;
+
+
+        private String m_primName;
+        private PhysicsVector _target_velocity;
+
+        public int m_eventsubscription = 0;
+        private CollisionEventUpdate CollisionEventsThisFrame = null;
+
+        public volatile bool childPrim = false;
+
+        private btVector3 tempPosition1;
+        private btVector3 tempPosition2;
+        private btVector3 tempPosition3;
+        private btVector3 tempSize1;
+        private btVector3 tempSize2;
+        private btVector3 tempLinearVelocity1;
+        private btVector3 tempLinearVelocity2;
+        private btVector3 tempAngularVelocity1;
+        private btVector3 tempAngularVelocity2;
+        private btVector3 tempInertia1;
+        private btVector3 tempInertia2;
+        private btQuaternion tempOrientation1;
+        private btQuaternion tempOrientation2;
+        private btMotionState tempMotionState1;
+        private btMotionState tempMotionState2;
+        private btMotionState tempMotionState3;
+        private btTransform tempTransform1;
+        private btTransform tempTransform2;
+        private btTransform tempTransform3;
+        private btTransform tempTransform4;
+        private btTriangleIndexVertexArray btshapeArray;
+
+        public btRigidBody Body;
+
+        public BulletDotNETPrim(String primName, BulletDotNETScene parent_scene, PhysicsVector pos, PhysicsVector size,
+                       Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical)
+        {
+            tempPosition1 = new btVector3(0, 0, 0);
+            tempPosition2 = new btVector3(0, 0, 0);
+            tempPosition3 = new btVector3(0, 0, 0);
+            tempSize1 = new btVector3(0, 0, 0);
+            tempSize2 = new btVector3(0, 0, 0);
+            tempLinearVelocity1 = new btVector3(0, 0, 0);
+            tempLinearVelocity2 = new btVector3(0, 0, 0);
+            tempAngularVelocity1 = new btVector3(0, 0, 0);
+            tempAngularVelocity2 = new btVector3(0, 0, 0);
+            tempInertia1 = new btVector3(0, 0, 0);
+            tempInertia2 = new btVector3(0, 0, 0);
+            tempOrientation1 = new btQuaternion(0,0,0,1);
+            tempOrientation2 = new btQuaternion(0, 0, 0, 1);
+            _parent_scene = parent_scene;
+            tempTransform1 = new btTransform(_parent_scene.QuatIdentity, _parent_scene.VectorZero);
+            tempTransform2 = new btTransform(_parent_scene.QuatIdentity, _parent_scene.VectorZero); ;
+            tempTransform3 = new btTransform(_parent_scene.QuatIdentity, _parent_scene.VectorZero); ;
+            tempTransform4 = new btTransform(_parent_scene.QuatIdentity, _parent_scene.VectorZero); ;
+
+            tempMotionState1 = new btDefaultMotionState(_parent_scene.TransZero);
+            tempMotionState2 = new btDefaultMotionState(_parent_scene.TransZero);
+            tempMotionState3 = new btDefaultMotionState(_parent_scene.TransZero);
+
+            _target_velocity = new PhysicsVector(0, 0, 0);
+            _velocity = new PhysicsVector();
+            _position = pos;
+            m_taintposition = pos;
+            PID_D = parent_scene.bodyPIDD;
+            PID_G = parent_scene.bodyPIDG;
+            m_density = parent_scene.geomDefaultDensity;
+            m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
+            body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
+
+            prim_geom = null;
+            Body = null;
+
+            if (size.X <= 0) size.X = 0.01f;
+            if (size.Y <= 0) size.Y = 0.01f;
+            if (size.Z <= 0) size.Z = 0.01f;
+
+            _size = size;
+            m_taintsize = _size;
+            _acceleration = new PhysicsVector();
+            m_rotationalVelocity = PhysicsVector.Zero;
+            _orientation = rotation;
+            m_taintrot = _orientation;
+            _mesh = mesh;
+            _pbs = pbs;
+
+            _parent_scene = parent_scene;
+
+            if (pos.Z < 0)
+                m_isphysical = false;
+            else
+            {
+                m_isphysical = pisPhysical;
+                // If we're physical, we need to be in the master space for now.
+                // linksets *should* be in a space together..  but are not currently
+            }
+            m_primName = primName;
+            m_taintadd = true;
+            _parent_scene.AddPhysicsActorTaint(this);
+
+        }
+
+        #region PhysicsActor overrides
+
+        public override bool Stopped
+        {
+            get { return _zeroFlag; }
+        }
+
+        public override PhysicsVector Size
+        {
+            get { return _size; }
+            set { _size = value; }
+        }
+
+        public override PrimitiveBaseShape Shape
+        {
+            set
+            {
+                _pbs = value;
+                m_taintshape = true;
+            }
+        }
+
+        public override uint LocalID
+        {
+            set
+            {
+                //m_log.Info("[PHYSICS]: Setting TrackerID: " + value);
+                m_localID = value;
+            }
+        }
+
+        public override bool Grabbed
+        {
+            set { return; }
+        }
+
+        public override bool Selected
+        {
+            set
+            {
+                // This only makes the object not collidable if the object
+                // is physical or the object is modified somehow *IN THE FUTURE*
+                // without this, if an avatar selects prim, they can walk right
+                // through it while it's selected
+                m_collisionscore = 0;
+                if ((m_isphysical && !_zeroFlag) || !value)
+                {
+                    m_taintselected = value;
+                    _parent_scene.AddPhysicsActorTaint(this);
+                }
+                else
+                {
+                    m_taintselected = value;
+                    m_isSelected = value;
+                }
+            }
+        }
+
+        public override void CrossingFailure()
+        {
+            m_crossingfailures++;
+            if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
+            {
+                base.RaiseOutOfBounds(_position);
+                return;
+            }
+            else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
+            {
+                m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName);
+            }
+        }
+        public override void link(PhysicsActor obj)
+        {
+            //TODO:
+        }
+
+        public override void delink()
+        {
+            //TODO:
+        }
+
+        public override void LockAngularMotion(PhysicsVector axis)
+        {
+            m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
+            m_taintAngularLock = new PhysicsVector(axis.X, axis.Y, axis.Z);
+        }
+
+        public override PhysicsVector Position
+        {
+            get { return _position; }
+
+            set
+            {
+                _position = value;
+                //m_log.Info("[PHYSICS]: " + _position.ToString());
+            }
+        }
+
+        public override float Mass
+        {
+            get { return CalculateMass(); }
+        }
+
+        public override PhysicsVector Force
+        {
+            //get { return PhysicsVector.Zero; }
+            get { return m_force; }
+            set { m_force = value; }
+        }
+
+        public override int VehicleType
+        {
+            get { return 0; }
+            set { return; }
+        }
+
+        public override void VehicleFloatParam(int param, float value)
+        {
+            //TODO:
+        }
+
+        public override void VehicleVectorParam(int param, PhysicsVector value)
+        {
+            //TODO:
+        }
+
+        public override void VehicleRotationParam(int param, Quaternion rotation)
+        {
+            //TODO:
+        }
+
+        public override void SetVolumeDetect(int param)
+        {
+            //TODO: GhostObject
+            m_isVolumeDetect = (param != 0);
+
+        }
+
+        public override PhysicsVector GeometricCenter
+        {
+            get { return PhysicsVector.Zero; }
+        }
+
+        public override PhysicsVector CenterOfMass
+        {
+            get { return PhysicsVector.Zero; }
+        }
+
+        public override PhysicsVector Velocity
+        {
+            get
+            {
+                // Averate previous velocity with the new one so
+                // client object interpolation works a 'little' better
+                PhysicsVector returnVelocity = new PhysicsVector();
+                returnVelocity.X = (m_lastVelocity.X + _velocity.X) / 2;
+                returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y) / 2;
+                returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z) / 2;
+                return returnVelocity;
+            }
+            set
+            {
+                _velocity = value;
+
+                m_taintVelocity = value;
+                _parent_scene.AddPhysicsActorTaint(this);
+            }
+        }
+
+        public override PhysicsVector Torque
+        {
+            get
+            {
+                if (!m_isphysical || Body.Handle == IntPtr.Zero)
+                    return new PhysicsVector(0, 0, 0);
+
+                return _torque;
+            }
+
+            set
+            {
+                m_taintTorque = value;
+                _parent_scene.AddPhysicsActorTaint(this);
+            }
+        }
+
+        public override float CollisionScore
+        {
+            get { return m_collisionscore; }
+            set { m_collisionscore = value; }
+        }
+
+        public override PhysicsVector Acceleration
+        {
+            get { return _acceleration; }
+        }
+
+        public override Quaternion Orientation
+        {
+            get { return _orientation; }
+            set { _orientation = value; }
+        }
+
+        public override int PhysicsActorType
+        {
+            get { return (int)ActorTypes.Prim; }
+            set { return; }
+        }
+
+        public override bool IsPhysical
+        {
+            get { return m_isphysical; }
+            set { m_isphysical = value; }
+        }
+
+        public override bool Flying
+        {
+            // no flying prims for you
+            get { return false; }
+            set { }
+        }
+
+        public override bool SetAlwaysRun
+        {
+            get { return false; }
+            set { return; }
+        }
+
+        public override bool ThrottleUpdates
+        {
+            get { return m_throttleUpdates; }
+            set { m_throttleUpdates = value; }
+        }
+
+        public override bool IsColliding
+        {
+            get { return iscolliding; }
+            set { iscolliding = value; }
+        }
+
+        public override bool CollidingGround
+        {
+            get { return false; }
+            set { return; }
+        }
+
+        public override bool CollidingObj
+        {
+            get { return false; }
+            set { return; }
+        }
+
+        public override bool FloatOnWater
+        {
+            set
+            {
+                m_taintCollidesWater = value;
+                _parent_scene.AddPhysicsActorTaint(this);
+            }
+        }
+
+        public override PhysicsVector RotationalVelocity
+        {
+            get
+            {
+                PhysicsVector pv = new PhysicsVector(0, 0, 0);
+                if (_zeroFlag)
+                    return pv;
+                m_lastUpdateSent = false;
+
+                if (m_rotationalVelocity.IsIdentical(pv, 0.2f))
+                    return pv;
+
+                return m_rotationalVelocity;
+            }
+            set { m_rotationalVelocity = value; }
+        }
+
+        public override bool Kinematic
+        {
+            get { return false; }
+            set { }
+        }
+
+        public override float Buoyancy
+        {
+            get { return m_buoyancy; }
+            set { m_buoyancy = value; }
+        }
+
+        public override PhysicsVector PIDTarget { set { m_PIDTarget = value; ; } }
+        public override bool PIDActive { set { m_usePID = value; } }
+        public override float PIDTau { set { m_PIDTau = value; } }
+
+        public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
+        public override bool PIDHoverActive { set { m_useHoverPID = value; } }
+        public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
+        public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
+
+
+        public override void AddForce(PhysicsVector force, bool pushforce)
+        {
+            m_forcelist.Add(force);
+            m_taintforce = true;
+            //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() +  " to prim at " + Position.ToString());
+        }
+
+        public override void AddAngularForce(PhysicsVector force, bool pushforce)
+        {
+            m_angularforcelist.Add(force);
+            m_taintaddangularforce = true;
+        }
+
+        public override void SetMomentum(PhysicsVector momentum)
+        {
+        }
+
+        public override void SubscribeEvents(int ms)
+        {
+            m_eventsubscription = ms;
+            _parent_scene.addCollisionEventReporting(this);
+        }
+
+        public override void UnSubscribeEvents()
+        {
+            _parent_scene.remCollisionEventReporting(this);
+            m_eventsubscription = 0;
+        }
+
+        public override bool SubscribedEvents()
+        {
+            return (m_eventsubscription > 0);
+        }
+
+        #endregion
+
+
+
+        internal void Dispose()
+        {
+            //TODO:
+            DisposeOfBody();
+            SetCollisionShape(null);
+
+            if (tempMotionState3 != null && tempMotionState3.Handle != IntPtr.Zero)
+            {
+                tempMotionState3.Dispose();
+                tempMotionState3 = null;
+            }
+
+            if (tempMotionState2 != null && tempMotionState2.Handle != IntPtr.Zero)
+            {
+                tempMotionState2.Dispose();
+                tempMotionState2 = null;
+            }
+
+            if (tempMotionState1 != null && tempMotionState1.Handle != IntPtr.Zero)
+            {
+                tempMotionState1.Dispose();
+                tempMotionState1 = null;
+            }
+
+            if (tempTransform4 != null && tempTransform4.Handle != IntPtr.Zero)
+            {
+                tempTransform4.Dispose();
+                tempTransform4 = null;
+            }
+
+            if (tempTransform3 != null && tempTransform3.Handle != IntPtr.Zero)
+            {
+                tempTransform3.Dispose();
+                tempTransform3 = null;
+            }
+
+            if (tempTransform2 != null && tempTransform2.Handle != IntPtr.Zero)
+            {
+                tempTransform2.Dispose();
+                tempTransform2 = null;
+            }
+
+            if (tempTransform1 != null && tempTransform1.Handle != IntPtr.Zero)
+            {
+                tempTransform1.Dispose();
+                tempTransform1 = null;
+            }
+
+            if (tempOrientation2 != null && tempOrientation2.Handle != IntPtr.Zero)
+            {
+                tempOrientation2.Dispose();
+                tempOrientation2 = null;
+            }
+
+            if (tempOrientation1 != null && tempOrientation1.Handle != IntPtr.Zero)
+            {
+                tempOrientation1.Dispose();
+                tempOrientation1 = null;
+            }
+
+            if (tempInertia1 != null && tempInertia1.Handle != IntPtr.Zero)
+            {
+                tempInertia1.Dispose();
+                tempInertia1 = null;
+            }
+
+            if (tempInertia2 != null && tempInertia2.Handle != IntPtr.Zero)
+            {
+                tempInertia2.Dispose();
+                tempInertia1 = null;
+            }
+            
+            
+            if (tempAngularVelocity2 != null && tempAngularVelocity2.Handle != IntPtr.Zero)
+            {
+                tempAngularVelocity2.Dispose();
+                tempAngularVelocity2 = null;
+            }
+
+            if (tempAngularVelocity1 != null && tempAngularVelocity1.Handle != IntPtr.Zero)
+            {
+                tempAngularVelocity1.Dispose();
+                tempAngularVelocity1 = null;
+            }
+
+            if (tempLinearVelocity2 != null && tempLinearVelocity2.Handle != IntPtr.Zero)
+            {
+                tempLinearVelocity2.Dispose();
+                tempLinearVelocity2 = null;
+            }
+
+            if (tempLinearVelocity1 != null && tempLinearVelocity1.Handle != IntPtr.Zero)
+            {
+                tempLinearVelocity1.Dispose();
+                tempLinearVelocity1 = null;
+            }
+
+            if (tempSize2 != null && tempSize2.Handle != IntPtr.Zero)
+            {
+                tempSize2.Dispose();
+                tempSize2 = null;
+            }
+
+            if (tempSize1 != null && tempSize1.Handle != IntPtr.Zero)
+            {
+                tempSize1.Dispose();
+                tempSize1 = null;
+            }
+
+            if (tempPosition3 != null && tempPosition3.Handle != IntPtr.Zero)
+            {
+                tempPosition3.Dispose();
+                tempPosition3 = null;
+            }
+
+            if (tempPosition2 != null && tempPosition2.Handle != IntPtr.Zero)
+            {
+                tempPosition2.Dispose();
+                tempPosition2 = null;
+            }
+
+            if (tempPosition1 != null && tempPosition1.Handle != IntPtr.Zero)
+            {
+                tempPosition1.Dispose();
+                tempPosition1 = null;
+            }
+
+        }
+
+
+
+        public void ProcessTaints(float timestep)
+        {
+            if (m_taintadd)
+            {
+                changeadd(timestep);
+            }
+
+            if (prim_geom.Handle == IntPtr.Zero)
+            {
+                CreateGeom(IntPtr.Zero, primMesh);
+                
+                if (IsPhysical)
+                    SetBody(Mass);
+                else
+                    SetBody(0);
+
+            }
+            
+            if (!_position.IsIdentical(m_taintposition, 0f))
+                changemove(timestep);
+
+            if (m_taintrot != _orientation)
+                rotate(timestep);
+            //
+
+            if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent))
+                changePhysicsStatus(timestep);
+            //
+
+            if (!_size.IsIdentical(m_taintsize, 0))
+                changesize(timestep);
+            //
+
+            if (m_taintshape)
+                changeshape(timestep);
+            //
+
+            if (m_taintforce)
+                changeAddForce(timestep);
+
+            if (m_taintaddangularforce)
+                changeAddAngularForce(timestep);
+
+            if (!m_taintTorque.IsIdentical(PhysicsVector.Zero, 0.001f))
+                changeSetTorque(timestep);
+
+            if (m_taintdisable)
+                changedisable(timestep);
+
+            if (m_taintselected != m_isSelected)
+                changeSelectedStatus(timestep);
+
+            if (!m_taintVelocity.IsIdentical(PhysicsVector.Zero, 0.001f))
+                changevelocity(timestep);
+
+            if (m_taintparent != _parent)
+                changelink(timestep);
+
+            if (m_taintCollidesWater != m_collidesWater)
+                changefloatonwater(timestep);
+
+            if (!m_angularlock.IsIdentical(m_taintAngularLock, 0))
+                changeAngularLock(timestep);
+            
+
+        }
+
+        #region Physics Scene Change Action routines
+
+        private void changeadd(float timestep)
+        {
+            if (_mesh == null)
+            {
+                if (_parent_scene.needsMeshing(_pbs))
+                {
+                    // Don't need to re-enable body..   it's done in SetMesh
+                    _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical);
+                    // createmesh returns null when it's a shape that isn't a cube.
+                }
+            }
+            CreateGeom(IntPtr.Zero, primMesh);
+
+            enableBody();
+            changeSelectedStatus(timestep);
+            m_taintadd = false;
+
+        }
+
+        private void changemove(float timestep)
+        {
+
+
+            tempTransform2 = Body.getWorldTransform();
+            btQuaternion quat = tempTransform2.getRotation();
+            tempPosition2.setValue(_position.X, _position.Y, _position.Z);
+            tempTransform2.Dispose();
+            tempTransform2 = new btTransform(quat, tempPosition2);
+            Body.setWorldTransform(tempTransform2);
+
+            changeSelectedStatus(timestep);
+
+            resetCollisionAccounting();
+            m_taintposition = _position;
+        }
+
+        private void rotate(float timestep)
+        {
+
+            tempTransform2 = Body.getWorldTransform();
+            tempOrientation2 = new btQuaternion(_orientation.X, _orientation.Y, _orientation.Z, _orientation.W);
+            tempTransform2.setRotation(tempOrientation2);
+            Body.setWorldTransform(tempTransform2);
+
+            resetCollisionAccounting();
+            m_taintrot = _orientation;
+        }
+
+        private void changePhysicsStatus(float timestep)
+        {
+            SetCollisionShape(prim_geom);
+            SetBody(Mass);
+            changeSelectedStatus(timestep);
+
+            resetCollisionAccounting();
+            m_taintPhysics = m_isphysical;
+        }
+
+        private void changesize(float timestep)
+        {
+            SetCollisionShape(null);
+            // Construction of new prim
+            if (_parent_scene.needsMeshing(_pbs))
+            {
+                float meshlod = _parent_scene.meshSculptLOD;
+
+                if (IsPhysical)
+                    meshlod = _parent_scene.MeshSculptphysicalLOD;
+                // Don't need to re-enable body..   it's done in SetMesh
+
+                IMesh mesh = null;
+
+                if (_parent_scene.needsMeshing(_pbs))
+                    mesh = _parent_scene.mesher.CreateMesh(SOPName, _pbs, _size, meshlod, IsPhysical);
+
+                //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
+
+                CreateGeom(IntPtr.Zero, mesh);
+
+
+            }
+            else
+            {
+                _mesh = null;
+                CreateGeom(IntPtr.Zero, _mesh);
+            }
+            
+            if (IsPhysical)
+                SetBody(Mass);
+            else
+                SetBody(0);
+
+            m_taintsize = _size;
+
+        }
+
+        private void changeshape(float timestep)
+        {
+            // Cleanup of old prim geometry and Bodies
+            if (IsPhysical && Body != null && Body.Handle != IntPtr.Zero)
+            {
+                if (childPrim)
+                {
+                    if (_parent != null)
+                    {
+                        BulletDotNETPrim parent = (BulletDotNETPrim)_parent;
+                        parent.ChildDelink(this);
+                    }
+                }
+                else
+                {
+                    disableBody();
+                }
+            }
+            try
+            {
+                SetCollisionShape(null);
+            }
+            catch (System.AccessViolationException)
+            {
+                //prim_geom = IntPtr.Zero;
+                m_log.Error("[PHYSICS]: PrimGeom dead");
+            }
+            
+            // we don't need to do space calculation because the client sends a position update also.
+            if (_size.X <= 0) _size.X = 0.01f;
+            if (_size.Y <= 0) _size.Y = 0.01f;
+            if (_size.Z <= 0) _size.Z = 0.01f;
+            // Construction of new prim
+
+            if (_parent_scene.needsMeshing(_pbs))
+            {
+                // Don't need to re-enable body..   it's done in SetMesh
+                float meshlod = _parent_scene.meshSculptLOD;
+
+                if (IsPhysical)
+                    meshlod = _parent_scene.MeshSculptphysicalLOD;
+
+                IMesh mesh = _parent_scene.mesher.CreateMesh(SOPName, _pbs, _size, meshlod, IsPhysical);
+                // createmesh returns null when it doesn't mesh.
+                CreateGeom(IntPtr.Zero, mesh);
+            }
+            else
+            {
+                _mesh = null;
+                CreateGeom(IntPtr.Zero, null);
+            }
+            tempPosition1.setValue(_position.X, _position.Y, _position.Z);
+            if (tempOrientation1.Handle != IntPtr.Zero)
+                tempOrientation1.Dispose();
+            tempOrientation1 = new btQuaternion(_orientation.X, Orientation.Y, _orientation.Z, _orientation.W);
+            if (tempTransform1 != null && tempTransform1.Handle != IntPtr.Zero)
+                tempTransform1.Dispose();
+            tempTransform1 = new btTransform(tempOrientation1, tempPosition1);
+
+            
+            
+
+            //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
+            if (IsPhysical)
+            {
+                SetBody(Mass);
+                // Re creates body on size.
+                // EnableBody also does setMass()
+               
+            }
+            else
+            {
+                SetBody(0);
+            }
+
+            changeSelectedStatus(timestep);
+            if (childPrim)
+            {
+                if (_parent is BulletDotNETPrim)
+                {
+                    BulletDotNETPrim parent = (BulletDotNETPrim)_parent;
+                    parent.ChildSetGeom(this);
+                }
+            }
+            resetCollisionAccounting();
+            m_taintshape = false;
+        }
+
+        private void resetCollisionAccounting()
+        {
+            m_collisionscore = 0;
+        }
+
+        private void ChildSetGeom(BulletDotNETPrim bulletDotNETPrim)
+        {
+            // TODO: throw new NotImplementedException();
+        }
+
+        private void changeAddForce(float timestep)
+        {
+            // TODO: throw new NotImplementedException();
+        }
+
+        private void changeAddAngularForce(float timestep)
+        {
+            // TODO: throw new NotImplementedException();
+        }
+
+        private void changeSetTorque(float timestep)
+        {
+            // TODO: throw new NotImplementedException();
+        }
+
+        private void changedisable(float timestep)
+        {
+            // TODO: throw new NotImplementedException();
+        }
+
+        private void changeSelectedStatus(float timestep)
+        {
+            // TODO: throw new NotImplementedException();
+        }
+
+        private void changevelocity(float timestep)
+        {
+            // TODO: throw new NotImplementedException();
+        }
+
+        private void changelink(float timestep)
+        {
+            // TODO: throw new NotImplementedException();
+        }
+
+        private void changefloatonwater(float timestep)
+        {
+            // TODO: throw new NotImplementedException();
+        }
+
+        private void changeAngularLock(float timestep)
+        {
+            // TODO: throw new NotImplementedException();
+        }
+        #endregion
+
+
+
+
+        internal void Move(float timestep)
+        {
+            //TODO:
+            float fx = 0;
+            float fy = 0;
+            float fz = 0;
+
+            if (IsPhysical && Body != null && Body.Handle != IntPtr.Zero && !m_isSelected)
+            {
+                float m_mass = CalculateMass();
+
+                fz = 0f;
+                //m_log.Info(m_collisionFlags.ToString());
+
+                if (m_buoyancy != 0)
+                {
+                    if (m_buoyancy > 0)
+                    {
+                        fz = (((-1 * _parent_scene.gravityz) * m_buoyancy) * m_mass);
+
+                        //d.Vector3 l_velocity = d.BodyGetLinearVel(Body);
+                        //m_log.Info("Using Buoyancy: " + buoyancy + " G: " + (_parent_scene.gravityz * m_buoyancy) + "mass:" + m_mass + "  Pos: " + Position.ToString());
+                    }
+                    else
+                    {
+                        fz = (-1 * (((-1 * _parent_scene.gravityz) * (-1 * m_buoyancy)) * m_mass));
+                    }
+                }
+
+                if (m_usePID)
+                {
+                    //if (!d.BodyIsEnabled(Body))
+                    //d.BodySetForce(Body, 0f, 0f, 0f);
+                    // If we're using the PID controller, then we have no gravity
+                    fz = (-1 * _parent_scene.gravityz) * m_mass;
+
+                    //  no lock; for now it's only called from within Simulate()
+
+                    // If the PID Controller isn't active then we set our force
+                    // calculating base velocity to the current position
+
+                    if ((m_PIDTau < 1) && (m_PIDTau != 0))
+                    {
+                        //PID_G = PID_G / m_PIDTau;
+                        m_PIDTau = 1;
+                    }
+
+                    if ((PID_G - m_PIDTau) <= 0)
+                    {
+                        PID_G = m_PIDTau + 1;
+                    }
+
+                    // TODO: NEED btVector3 for Linear Velocity
+                    // NEED btVector3 for Position
+
+                    PhysicsVector pos = new PhysicsVector(0, 0, 0); //TODO: Insert values gotten from bullet
+                    PhysicsVector vel = new PhysicsVector(0, 0, 0);
+
+                    _target_velocity =
+                        new PhysicsVector(
+                            (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
+                            (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
+                            (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
+                            );
+
+                    if (_target_velocity.IsIdentical(PhysicsVector.Zero, 0.1f))
+                    {
+
+                        /* TODO: Do Bullet equiv
+                         * 
+                        d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
+                        d.BodySetLinearVel(Body, 0, 0, 0);
+                        d.BodyAddForce(Body, 0, 0, fz);
+                        return;
+                        */
+                    }
+                    else
+                    {
+                        _zeroFlag = false;
+
+                        fx = ((_target_velocity.X) - vel.X) * (PID_D);
+                        fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
+                        fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
+
+                    }
+
+                }
+
+                if (m_useHoverPID && !m_usePID)
+                {
+                    // If we're using the PID controller, then we have no gravity
+                    fz = (-1 * _parent_scene.gravityz) * m_mass;
+
+                    //  no lock; for now it's only called from within Simulate()
+
+                    // If the PID Controller isn't active then we set our force
+                    // calculating base velocity to the current position
+
+                    if ((m_PIDTau < 1))
+                    {
+                        PID_G = PID_G / m_PIDTau;
+                    }
+
+                    if ((PID_G - m_PIDTau) <= 0)
+                    {
+                        PID_G = m_PIDTau + 1;
+                    }
+                    PhysicsVector pos = new PhysicsVector(0, 0, 0); //TODO: Insert values gotten from bullet
+                    PhysicsVector vel = new PhysicsVector(0, 0, 0);
+
+                    // determine what our target height really is based on HoverType
+                    switch (m_PIDHoverType)
+                    {
+                        case PIDHoverType.Absolute:
+                            m_targetHoverHeight = m_PIDHoverHeight;
+                            break;
+                        case PIDHoverType.Ground:
+                            m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
+                            m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
+                            break;
+                        case PIDHoverType.GroundAndWater:
+                            m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
+                            m_waterHeight = _parent_scene.GetWaterLevel();
+                            if (m_groundHeight > m_waterHeight)
+                            {
+                                m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
+                            }
+                            else
+                            {
+                                m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
+                            }
+                            break;
+                        case PIDHoverType.Water:
+                            m_waterHeight = _parent_scene.GetWaterLevel();
+                            m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
+                            break;
+                    }
+
+
+                    _target_velocity =
+                        new PhysicsVector(0.0f, 0.0f,
+                            (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
+                            );
+
+                    //  if velocity is zero, use position control; otherwise, velocity control
+
+                    if (_target_velocity.IsIdentical(PhysicsVector.Zero, 0.1f))
+                    {
+
+                        /* TODO: Do Bullet Equiv
+                        d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
+                        d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
+                        d.BodyAddForce(Body, 0, 0, fz);
+                        */
+                        return;
+                    }
+                    else
+                    {
+                        _zeroFlag = false;
+
+                        // We're flying and colliding with something
+                        fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
+                    }
+                }
+
+                fx *= m_mass;
+                fy *= m_mass;
+                //fz *= m_mass;
+
+                fx += m_force.X;
+                fy += m_force.Y;
+                fz += m_force.Z;
+
+                //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
+                if (fx != 0 || fy != 0 || fz != 0)
+                {
+                    /*
+                     * TODO: Do Bullet Equiv
+                    if (!d.BodyIsEnabled(Body))
+                    {
+                        d.BodySetLinearVel(Body, 0f, 0f, 0f);
+                        d.BodySetForce(Body, 0, 0, 0);
+                        enableBodySoft();
+                    }
+                    */
+                    // 35x10 = 350n times the mass per second applied maximum.
+
+                    float nmax = 35f * m_mass;
+                    float nmin = -35f * m_mass;
+
+
+                    if (fx > nmax)
+                        fx = nmax;
+                    if (fx < nmin)
+                        fx = nmin;
+                    if (fy > nmax)
+                        fy = nmax;
+                    if (fy < nmin)
+                        fy = nmin;
+
+                    // TODO: Do Bullet Equiv
+                    // d.BodyAddForce(Body, fx, fy, fz);
+                }
+            }
+            else
+            {
+                // _zeroPosition = d.BodyGetPosition(Body);
+                return;
+            }
+        }
+
+
+
+
+        #region Mass Calculation
+
+        private float CalculateMass()
+        {
+            float volume = 0;
+
+            // No material is passed to the physics engines yet..  soo..
+            // we're using the m_density constant in the class definition
+
+            float returnMass = 0;
+
+            switch (_pbs.ProfileShape)
+            {
+                case ProfileShape.Square:
+                    // Profile Volume
+
+                    volume = _size.X * _size.Y * _size.Z;
+
+                    // If the user has 'hollowed out'
+                    // ProfileHollow is one of those 0 to 50000 values :P
+                    // we like percentages better..   so turning into a percentage
+
+                    if (((float)_pbs.ProfileHollow / 50000f) > 0.0)
+                    {
+                        float hollowAmount = (float)_pbs.ProfileHollow / 50000f;
+
+                        // calculate the hollow volume by it's shape compared to the prim shape
+                        float hollowVolume = 0;
+                        switch (_pbs.HollowShape)
+                        {
+                            case HollowShape.Square:
+                            case HollowShape.Same:
+                                // Cube Hollow volume calculation
+                                float hollowsizex = _size.X * hollowAmount;
+                                float hollowsizey = _size.Y * hollowAmount;
+                                float hollowsizez = _size.Z * hollowAmount;
+                                hollowVolume = hollowsizex * hollowsizey * hollowsizez;
+                                break;
+
+                            case HollowShape.Circle:
+                                // Hollow shape is a perfect cyllinder in respect to the cube's scale
+                                // Cyllinder hollow volume calculation
+                                float hRadius = _size.X / 2;
+                                float hLength = _size.Z;
+
+                                // pi * r2 * h
+                                hollowVolume = ((float)(Math.PI * Math.Pow(hRadius, 2) * hLength) * hollowAmount);
+                                break;
+
+                            case HollowShape.Triangle:
+                                // Equilateral Triangular Prism volume hollow calculation
+                                // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y
+
+                                float aLength = _size.Y;
+                                // 1/2 abh
+                                hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount);
+                                break;
+
+                            default:
+                                hollowVolume = 0;
+                                break;
+                        }
+                        volume = volume - hollowVolume;
+                    }
+
+                    break;
+                case ProfileShape.Circle:
+                    if (_pbs.PathCurve == (byte)Extrusion.Straight)
+                    {
+                        // Cylinder
+                        float volume1 = (float)(Math.PI * Math.Pow(_size.X / 2, 2) * _size.Z);
+                        float volume2 = (float)(Math.PI * Math.Pow(_size.Y / 2, 2) * _size.Z);
+
+                        // Approximating the cylinder's irregularity.
+                        if (volume1 > volume2)
+                        {
+                            volume = (float)volume1 - (volume1 - volume2);
+                        }
+                        else if (volume2 > volume1)
+                        {
+                            volume = (float)volume2 - (volume2 - volume1);
+                        }
+                        else
+                        {
+                            // Regular cylinder
+                            volume = volume1;
+                        }
+                    }
+                    else
+                    {
+                        // We don't know what the shape is yet, so use default
+                        volume = _size.X * _size.Y * _size.Z;
+                    }
+                    // If the user has 'hollowed out'
+                    // ProfileHollow is one of those 0 to 50000 values :P
+                    // we like percentages better..   so turning into a percentage
+
+                    if (((float)_pbs.ProfileHollow / 50000f) > 0.0)
+                    {
+                        float hollowAmount = (float)_pbs.ProfileHollow / 50000f;
+
+                        // calculate the hollow volume by it's shape compared to the prim shape
+                        float hollowVolume = 0;
+                        switch (_pbs.HollowShape)
+                        {
+                            case HollowShape.Same:
+                            case HollowShape.Circle:
+                                // Hollow shape is a perfect cyllinder in respect to the cube's scale
+                                // Cyllinder hollow volume calculation
+                                float hRadius = _size.X / 2;
+                                float hLength = _size.Z;
+
+                                // pi * r2 * h
+                                hollowVolume = ((float)(Math.PI * Math.Pow(hRadius, 2) * hLength) * hollowAmount);
+                                break;
+
+                            case HollowShape.Square:
+                                // Cube Hollow volume calculation
+                                float hollowsizex = _size.X * hollowAmount;
+                                float hollowsizey = _size.Y * hollowAmount;
+                                float hollowsizez = _size.Z * hollowAmount;
+                                hollowVolume = hollowsizex * hollowsizey * hollowsizez;
+                                break;
+
+                            case HollowShape.Triangle:
+                                // Equilateral Triangular Prism volume hollow calculation
+                                // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y
+
+                                float aLength = _size.Y;
+                                // 1/2 abh
+                                hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount);
+                                break;
+
+                            default:
+                                hollowVolume = 0;
+                                break;
+                        }
+                        volume = volume - hollowVolume;
+                    }
+                    break;
+
+                case ProfileShape.HalfCircle:
+                    if (_pbs.PathCurve == (byte)Extrusion.Curve1)
+                    {
+                        if (_size.X == _size.Y && _size.Z == _size.X)
+                        {
+                            // regular sphere
+                            // v = 4/3 * pi * r^3
+                            float sradius3 = (float)Math.Pow((_size.X / 2), 3);
+                            volume = (float)((4 / 3f) * Math.PI * sradius3);
+                        }
+                        else
+                        {
+                            // we treat this as a box currently
+                            volume = _size.X * _size.Y * _size.Z;
+                        }
+                    }
+                    else
+                    {
+                        // We don't know what the shape is yet, so use default
+                        volume = _size.X * _size.Y * _size.Z;
+                    }
+                    break;
+
+                case ProfileShape.EquilateralTriangle:
+                    /*
+                        v = (abs((xB*yA-xA*yB)+(xC*yB-xB*yC)+(xA*yC-xC*yA))/2) * h
+
+                        // seed mesh
+                        Vertex MM = new Vertex(-0.25f, -0.45f, 0.0f);
+                        Vertex PM = new Vertex(+0.5f, 0f, 0.0f);
+                        Vertex PP = new Vertex(-0.25f, +0.45f, 0.0f);
+                     */
+                    float xA = -0.25f * _size.X;
+                    float yA = -0.45f * _size.Y;
+
+                    float xB = 0.5f * _size.X;
+                    float yB = 0;
+
+                    float xC = -0.25f * _size.X;
+                    float yC = 0.45f * _size.Y;
+
+                    volume = (float)((Math.Abs((xB * yA - xA * yB) + (xC * yB - xB * yC) + (xA * yC - xC * yA)) / 2) * _size.Z);
+
+                    // If the user has 'hollowed out'
+                    // ProfileHollow is one of those 0 to 50000 values :P
+                    // we like percentages better..   so turning into a percentage
+                    float fhollowFactor = ((float)_pbs.ProfileHollow / 1.9f);
+                    if (((float)fhollowFactor / 50000f) > 0.0)
+                    {
+                        float hollowAmount = (float)fhollowFactor / 50000f;
+
+                        // calculate the hollow volume by it's shape compared to the prim shape
+                        float hollowVolume = 0;
+                        switch (_pbs.HollowShape)
+                        {
+                            case HollowShape.Same:
+                            case HollowShape.Triangle:
+                                // Equilateral Triangular Prism volume hollow calculation
+                                // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y
+
+                                float aLength = _size.Y;
+                                // 1/2 abh
+                                hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount);
+                                break;
+
+                            case HollowShape.Square:
+                                // Cube Hollow volume calculation
+                                float hollowsizex = _size.X * hollowAmount;
+                                float hollowsizey = _size.Y * hollowAmount;
+                                float hollowsizez = _size.Z * hollowAmount;
+                                hollowVolume = hollowsizex * hollowsizey * hollowsizez;
+                                break;
+
+                            case HollowShape.Circle:
+                                // Hollow shape is a perfect cyllinder in respect to the cube's scale
+                                // Cyllinder hollow volume calculation
+                                float hRadius = _size.X / 2;
+                                float hLength = _size.Z;
+
+                                // pi * r2 * h
+                                hollowVolume = ((float)((Math.PI * Math.Pow(hRadius, 2) * hLength) / 2) * hollowAmount);
+                                break;
+
+                            default:
+                                hollowVolume = 0;
+                                break;
+                        }
+                        volume = volume - hollowVolume;
+                    }
+                    break;
+
+                default:
+                    // we don't have all of the volume formulas yet so
+                    // use the common volume formula for all
+                    volume = _size.X * _size.Y * _size.Z;
+                    break;
+            }
+
+            // Calculate Path cut effect on volume
+            // Not exact, in the triangle hollow example
+            // They should never be zero or less then zero..
+            // we'll ignore it if it's less then zero
+
+            // ProfileEnd and ProfileBegin are values
+            // from 0 to 50000
+
+            // Turning them back into percentages so that I can cut that percentage off the volume
+
+            float PathCutEndAmount = _pbs.ProfileEnd;
+            float PathCutStartAmount = _pbs.ProfileBegin;
+            if (((PathCutStartAmount + PathCutEndAmount) / 50000f) > 0.0f)
+            {
+                float pathCutAmount = ((PathCutStartAmount + PathCutEndAmount) / 50000f);
+
+                // Check the return amount for sanity
+                if (pathCutAmount >= 0.99f)
+                    pathCutAmount = 0.99f;
+
+                volume = volume - (volume * pathCutAmount);
+            }
+            UInt16 taperX = _pbs.PathScaleX;
+            UInt16 taperY = _pbs.PathScaleY;
+            float taperFactorX = 0;
+            float taperFactorY = 0;
+
+            // Mass = density * volume
+            if (taperX != 100)
+            {
+                if (taperX > 100)
+                {
+                    taperFactorX = 1.0f - ((float)taperX / 200);
+                    //m_log.Warn("taperTopFactorX: " + extr.taperTopFactorX.ToString());
+                }
+                else
+                {
+                    taperFactorX = 1.0f - ((100 - (float)taperX) / 100);
+                    //m_log.Warn("taperBotFactorX: " + extr.taperBotFactorX.ToString());
+                }
+                volume = (float)volume * ((taperFactorX / 3f) + 0.001f);
+            }
+
+            if (taperY != 100)
+            {
+                if (taperY > 100)
+                {
+                    taperFactorY = 1.0f - ((float)taperY / 200);
+                    //m_log.Warn("taperTopFactorY: " + extr.taperTopFactorY.ToString());
+                }
+                else
+                {
+                    taperFactorY = 1.0f - ((100 - (float)taperY) / 100);
+                    //m_log.Warn("taperBotFactorY: " + extr.taperBotFactorY.ToString());
+                }
+                volume = (float)volume * ((taperFactorY / 3f) + 0.001f);
+            }
+            returnMass = m_density * volume;
+            if (returnMass <= 0) returnMass = 0.0001f;//ckrinke: Mass must be greater then zero.
+
+
+
+            // Recursively calculate mass
+            bool HasChildPrim = false;
+            lock (childrenPrim)
+            {
+                if (childrenPrim.Count > 0)
+                {
+                    HasChildPrim = true;
+                }
+
+            }
+            if (HasChildPrim)
+            {
+                BulletDotNETPrim[] childPrimArr = new BulletDotNETPrim[0];
+
+                lock (childrenPrim)
+                    childPrimArr = childrenPrim.ToArray();
+
+                for (int i = 0; i < childPrimArr.Length; i++)
+                {
+                    if (childPrimArr[i] != null && !childPrimArr[i].m_taintremove)
+                        returnMass += childPrimArr[i].CalculateMass();
+                    // failsafe, this shouldn't happen but with OpenSim, you never know :)
+                    if (i > 256)
+                        break;
+                }
+            }
+
+
+
+
+
+            return returnMass;
+        }
+
+        #endregion
+
+
+        public void CreateGeom(IntPtr m_targetSpace, IMesh p_mesh)
+        {
+            if (p_mesh != null)
+            {
+                _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical);
+                setMesh(_parent_scene, _mesh);
+                
+            }
+            else
+            {
+                if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1)
+                {
+                    if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z)
+                    {
+                        if (((_size.X / 2f) > 0f))
+                        {
+                            //SetGeom to a Regular Sphere
+                            tempSize1.setValue(_size.X * 0.5f, _size.Y * 0.5f, _size.Z * 0.5f);
+                            SetCollisionShape(new btSphereShape(_size.X*0.5f));
+                        }
+                        else
+                        {
+                            // uses halfextents
+                            tempSize1.setValue(_size.X*0.5f, _size.Y*0.5f, _size.Z*0.5f);
+                            SetCollisionShape(new btBoxShape(tempSize1));
+                        }
+                    }
+                    else
+                    {
+                        // uses halfextents
+                        tempSize1.setValue(_size.X * 0.5f, _size.Y * 0.5f, _size.Z * 0.5f);
+                        SetCollisionShape(new btBoxShape(tempSize1));
+                    }
+
+                }
+                else
+                {
+                    // uses halfextents
+                    tempSize1.setValue(_size.X * 0.5f, _size.Y * 0.5f, _size.Z * 0.5f);
+                    SetCollisionShape(new btBoxShape(tempSize1));
+                }
+            }
+        }
+
+        private void setMesh(BulletDotNETScene _parent_scene, IMesh mesh)
+        {
+            // TODO: Set Collision Body Mesh
+            // This sleeper is there to moderate how long it takes between
+            // setting up the mesh and pre-processing it when we get rapid fire mesh requests on a single object
+
+            Thread.Sleep(10);
+
+            //Kill Body so that mesh can re-make the geom
+            if (IsPhysical && Body != null && Body.Handle != IntPtr.Zero)
+            {
+                if (childPrim)
+                {
+                    if (_parent != null)
+                    {
+                        BulletDotNETPrim parent = (BulletDotNETPrim)_parent;
+                        parent.ChildDelink(this);
+                    }
+                }
+                else
+                {
+                    disableBody();
+                }
+            }
+
+            IMesh oldMesh = primMesh;
+
+            primMesh = mesh;
+            
+            float[] vertexList = primMesh.getVertexListAsFloatLocked(); // Note, that vertextList is pinned in memory
+            int[] indexList = primMesh.getIndexListAsIntLocked(); // Also pinned, needs release after usage
+            //Array.Reverse(indexList);
+            primMesh.releaseSourceMeshData(); // free up the original mesh data to save memory
+
+            int VertexCount = vertexList.GetLength(0) / 3;
+            int IndexCount = indexList.GetLength(0);
+
+            if (btshapeArray != null && btshapeArray.Handle != IntPtr.Zero)
+                btshapeArray.Dispose();
+            //Array.Reverse(indexList);
+            btshapeArray = new btTriangleIndexVertexArray(IndexCount / 3, indexList, (3 * sizeof(int)),
+                                                                                     VertexCount, vertexList, 3*sizeof (float));
+            SetCollisionShape(new btGImpactMeshShape(btshapeArray));
+            //((btGImpactMeshShape) prim_geom).updateBound();
+            ((btGImpactMeshShape)prim_geom).setLocalScaling(new btVector3(1,1, 1));
+            ((btGImpactMeshShape)prim_geom).updateBound();
+            _parent_scene.SetUsingGImpact();
+            if (oldMesh != null)
+            {
+                oldMesh.releasePinned();
+                oldMesh = null;
+            }
+
+        }
+
+        private void SetCollisionShape(btCollisionShape shape)
+        {
+            if (shape == null)
+                m_log.Debug("[PHYSICS]:SetShape!Null");
+            else
+                m_log.Debug("[PHYSICS]:SetShape!");
+
+            if (Body != null)
+            {
+                DisposeOfBody();
+            }
+
+            if (prim_geom != null)
+            {
+                prim_geom.Dispose();
+                prim_geom = null;
+            }
+            prim_geom = shape;
+           
+            //Body.set
+        }
+
+        public void SetBody(float mass)
+        {
+            m_log.DebugFormat("[PHYSICS]: SetBody! {0}",mass);
+            if (Body != null && Body.Handle != IntPtr.Zero)
+            {
+                DisposeOfBody();
+            }
+
+            if (tempMotionState1 != null && tempMotionState1.Handle != IntPtr.Zero)
+                tempMotionState1.Dispose();
+            if (tempTransform2 != null && tempTransform2.Handle != IntPtr.Zero)
+                tempTransform2.Dispose();
+            if (tempOrientation2 != null && tempOrientation2.Handle != IntPtr.Zero)
+                tempOrientation2.Dispose();
+
+            if (tempPosition2 != null && tempPosition2.Handle != IntPtr.Zero)
+                tempPosition2.Dispose();
+
+            tempOrientation2 = new btQuaternion(_orientation.X, _orientation.Y, _orientation.Z, _orientation.W);
+            tempPosition2 = new btVector3(_position.X, _position.Y, _position.Z);
+            tempTransform2 = new btTransform(tempOrientation2, tempPosition2);
+            tempMotionState1 = new btDefaultMotionState(tempTransform2, _parent_scene.TransZero);
+            if (tempInertia1 != null && tempInertia1.Handle != IntPtr.Zero)
+                tempInertia1.Dispose();
+            tempInertia1 = new btVector3(0, 0, 0);
+            if (prim_geom.Handle == IntPtr.Zero)
+            {
+                m_log.Warn("[PHYSICS]:PrimGeom is Disposed!");
+                CreateGeom(IntPtr.Zero, primMesh);
+
+            }
+            prim_geom.calculateLocalInertia(mass, tempInertia1);
+
+            if (mass == 0)
+                Body = new btRigidBody(mass, tempMotionState1, prim_geom);
+            else
+                Body = new btRigidBody(mass, tempMotionState1, prim_geom, tempInertia1);
+
+            if (prim_geom is btGImpactMeshShape)
+            {
+                ((btGImpactMeshShape) prim_geom).setLocalScaling(new btVector3(1, 1, 1));
+                ((btGImpactMeshShape) prim_geom).updateBound();
+            }
+            _parent_scene.AddPrimToScene(this);
+        }
+
+        private void DisposeOfBody()
+        {
+            if (Body != null)
+            {
+                if (Body.Handle != IntPtr.Zero)
+                {
+                    _parent_scene.removeFromWorld(this,Body);
+                    Body.Dispose();
+                }
+                Body = null;
+                // TODO: dispose parts that make up body
+            }
+        }
+
+        private void ChildDelink(BulletDotNETPrim pPrim)
+        {
+            // Okay, we have a delinked child..   need to rebuild the body.
+            lock (childrenPrim)
+            {
+                foreach (BulletDotNETPrim prm in childrenPrim)
+                {
+                    prm.childPrim = true;
+                    prm.disableBody();
+
+                }
+            }
+            disableBody();
+
+            lock (childrenPrim)
+            {
+                childrenPrim.Remove(pPrim);
+            }
+
+
+
+
+            if (Body != null && Body.Handle != IntPtr.Zero)
+            {
+                _parent_scene.remActivePrim(this);
+            }
+
+
+
+            lock (childrenPrim)
+            {
+                foreach (BulletDotNETPrim prm in childrenPrim)
+                {
+                    ParentPrim(prm);
+                }
+            }
+
+        }
+
+        private void ParentPrim(BulletDotNETPrim prm)
+        {
+            // TODO: Parent Linking algorithm.   Use btComplexObject
+        }
+
+        public void disableBody()
+        {
+            //this kills the body so things like 'mesh' can re-create it.
+            lock (this)
+            {
+                if (!childPrim)
+                {
+                    if (Body != null && Body.Handle != IntPtr.Zero)
+                    {
+                        _parent_scene.remActivePrim(this);
+
+                        m_collisionCategories &= ~CollisionCategories.Body;
+                        m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
+
+                        if (prim_geom != null && prim_geom.Handle != IntPtr.Zero)
+                        {
+                            // TODO: Set Category bits and Flags
+                        }
+
+                        // TODO: destroy body
+                        DisposeOfBody();
+
+                        lock (childrenPrim)
+                        {
+                            if (childrenPrim.Count > 0)
+                            {
+                                foreach (BulletDotNETPrim prm in childrenPrim)
+                                {
+                                    _parent_scene.remActivePrim(prm);
+                                    prm.DisposeOfBody();
+                                    prm.SetCollisionShape(null);
+                                }
+                            }
+
+                        }
+
+                        DisposeOfBody();
+                    }
+                }
+                else
+                {
+                    _parent_scene.remActivePrim(this);
+                    m_collisionCategories &= ~CollisionCategories.Body;
+                    m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
+
+                    if (prim_geom != null && prim_geom.Handle != IntPtr.Zero)
+                    {
+                        // TODO: Set Category bits and Flags
+                    }
+
+                    DisposeOfBody();
+                }
+                
+            }
+            m_disabled = true;
+            m_collisionscore = 0;
+        }
+
+        public void disableBodySoft()
+        {
+            m_disabled = true;
+
+            if (m_isphysical && Body.Handle != IntPtr.Zero)
+            {
+                Body.clearForces();
+                Body.forceActivationState(0);
+                
+            }
+
+        }
+
+        public void enableBodySoft()
+        {
+            if (!childPrim)
+            {
+                if (m_isphysical && Body.Handle != IntPtr.Zero)
+                {
+                    Body.clearForces();
+                    Body.forceActivationState(1);
+                }
+                m_disabled = false;
+            }
+        }
+
+        public void enableBody()
+        {
+            if (!childPrim)
+            {
+                //SetCollisionShape(prim_geom);
+                if (IsPhysical)
+                    SetBody(Mass);
+                else
+                    SetBody(0);
+                
+                // TODO: Set Collision Category Bits and Flags
+                // TODO: Set Auto Disable data
+
+                m_interpenetrationcount = 0;
+                m_collisionscore = 0;
+                m_disabled = false;
+                // The body doesn't already have a finite rotation mode set here
+                if ((!m_angularlock.IsIdentical(PhysicsVector.Zero, 0)) && _parent == null)
+                {
+                    // TODO: Create Angular Motor on Axis Lock!
+                }
+                _parent_scene.addActivePrim(this);
+            }
+        }
+
+        public void UpdatePositionAndVelocity()
+        {
+            if (_parent == null)
+            {
+                PhysicsVector pv = new PhysicsVector(0, 0, 0);
+                bool lastZeroFlag = _zeroFlag;
+                if (tempPosition2 != null && tempPosition2.Handle != IntPtr.Zero)
+                    tempPosition2.Dispose();
+                if (tempTransform3 != null && tempTransform3.Handle != IntPtr.Zero)
+                    tempTransform3.Dispose();
+
+                if (tempOrientation2 != null && tempOrientation2.Handle != IntPtr.Zero)
+                    tempOrientation2.Dispose();
+
+                if (tempAngularVelocity1 != null && tempAngularVelocity1.Handle != IntPtr.Zero)
+                    tempAngularVelocity1.Dispose();
+
+                if (tempLinearVelocity1 != null && tempLinearVelocity1.Handle != IntPtr.Zero)
+                    tempLinearVelocity1.Dispose();
+
+
+
+                tempTransform3 = Body.getInterpolationWorldTransform();
+                tempPosition2 = tempTransform3.getOrigin(); // vec
+                tempOrientation2 = tempTransform3.getRotation(); // ori
+                tempAngularVelocity1 = Body.getInterpolationAngularVelocity(); //rotvel
+                tempLinearVelocity1 = Body.getInterpolationLinearVelocity(); // vel
+
+                _torque.setValues(tempAngularVelocity1.getX(), tempAngularVelocity1.getX(), tempAngularVelocity1.getZ());
+                PhysicsVector l_position = new PhysicsVector();
+                Quaternion l_orientation = new Quaternion();
+                m_lastposition = _position;
+                m_lastorientation = _orientation;
+
+                l_position.X = tempPosition2.getX();
+                l_position.Y = tempPosition2.getY();
+                l_position.Z = tempPosition2.getZ();
+                l_orientation.X = tempOrientation2.getX();
+                l_orientation.Y = tempOrientation2.getY();
+                l_orientation.Z = tempOrientation2.getZ();
+                l_orientation.W = tempOrientation2.getW();
+
+                if (l_position.X > 255.95f || l_position.X < 0f || l_position.Y > 255.95f || l_position.Y < 0f)
+                {
+                    //base.RaiseOutOfBounds(l_position);
+
+                    if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds)
+                    {
+                        _position = l_position;
+                        //_parent_scene.remActivePrim(this);
+                        if (_parent == null)
+                            base.RequestPhysicsterseUpdate();
+                        return;
+                    }
+                    else
+                    {
+                        if (_parent == null)
+                            base.RaiseOutOfBounds(l_position);
+                        return;
+                    }
+                }
+
+                if (l_position.Z < -200000f)
+                {
+                    // This is so prim that get lost underground don't fall forever and suck up
+                    //
+                    // Sim resources and memory.
+                    // Disables the prim's movement physics....
+                    // It's a hack and will generate a console message if it fails.
+
+                    //IsPhysical = false;
+                    //if (_parent == null)
+                        //base.RaiseOutOfBounds(_position);
+
+                    _acceleration.X = 0;
+                    _acceleration.Y = 0;
+                    _acceleration.Z = 0;
+
+                    _velocity.X = 0;
+                    _velocity.Y = 0;
+                    _velocity.Z = 0;
+                    m_rotationalVelocity.X = 0;
+                    m_rotationalVelocity.Y = 0;
+                    m_rotationalVelocity.Z = 0;
+
+                    if (_parent == null)
+                        base.RequestPhysicsterseUpdate();
+
+                    m_throttleUpdates = false;
+                    throttleCounter = 0;
+                    _zeroFlag = true;
+                    //outofBounds = true;
+                }
+
+                if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
+                    && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
+                    && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)
+                    && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01 ))
+                {
+                    _zeroFlag = true;
+                    m_throttleUpdates = false;
+                }
+                else
+                {
+                    //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString());
+                    _zeroFlag = false;
+                }
+
+                if (_zeroFlag)
+                {
+                    _velocity.X = 0.0f;
+                    _velocity.Y = 0.0f;
+                    _velocity.Z = 0.0f;
+
+                    _acceleration.X = 0;
+                    _acceleration.Y = 0;
+                    _acceleration.Z = 0;
+
+                    //_orientation.w = 0f;
+                    //_orientation.X = 0f;
+                    //_orientation.Y = 0f;
+                    //_orientation.Z = 0f;
+                    m_rotationalVelocity.X = 0;
+                    m_rotationalVelocity.Y = 0;
+                    m_rotationalVelocity.Z = 0;
+                    if (!m_lastUpdateSent)
+                    {
+                        m_throttleUpdates = false;
+                        throttleCounter = 0;
+                        m_rotationalVelocity = pv;
+
+                        if (_parent == null)
+                            base.RequestPhysicsterseUpdate();
+
+                        m_lastUpdateSent = true;
+                    }
+                }
+                else
+                {
+                    if (lastZeroFlag != _zeroFlag)
+                    {
+                        if (_parent == null)
+                            base.RequestPhysicsterseUpdate();
+                    }
+
+                    m_lastVelocity = _velocity;
+
+                    _position = l_position;
+
+                    _velocity.X = tempLinearVelocity1.getX();
+                    _velocity.Y = tempLinearVelocity1.getY();
+                    _velocity.Z = tempLinearVelocity1.getZ();
+
+                    _acceleration = ((_velocity - m_lastVelocity) / 0.1f);
+                    _acceleration = new PhysicsVector(_velocity.X - m_lastVelocity.X / 0.1f, _velocity.Y - m_lastVelocity.Y / 0.1f, _velocity.Z - m_lastVelocity.Z / 0.1f);
+                    //m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString());
+
+                    if (_velocity.IsIdentical(pv, 0.5f))
+                    {
+                        m_rotationalVelocity = pv;
+                    }
+                    else
+                    {
+                        
+                        m_rotationalVelocity.setValues(tempAngularVelocity1.getX(), tempAngularVelocity1.getY(), tempAngularVelocity1.getZ());
+                    }
+
+                    //m_log.Debug("ODE: " + m_rotationalVelocity.ToString());
+
+                    _orientation.X = l_orientation.X;
+                    _orientation.Y = l_orientation.Y;
+                    _orientation.Z = l_orientation.Z;
+                    _orientation.W = l_orientation.W;
+                    m_lastUpdateSent = false;
+                    
+                    //if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
+                    //{
+                        if (_parent == null)
+                            base.RequestPhysicsterseUpdate();
+                   // }
+                   // else
+                   // {
+                   //     throttleCounter++;
+                    //}
+                   
+                }
+                m_lastposition = l_position;
+            }
+            else
+            {
+                // Not a body..   so Make sure the client isn't interpolating
+                _velocity.X = 0;
+                _velocity.Y = 0;
+                _velocity.Z = 0;
+
+                _acceleration.X = 0;
+                _acceleration.Y = 0;
+                _acceleration.Z = 0;
+
+                m_rotationalVelocity.X = 0;
+                m_rotationalVelocity.Y = 0;
+                m_rotationalVelocity.Z = 0;
+                _zeroFlag = true;
+            }
+        }
+
+
+        internal void setPrimForRemoval()
+        {
+            m_taintremove = true;
+        }
+    }
+}
+