public void removeRigidBody(btRigidBody body)
        {
            if (m_disposed)
                throw new ObjectDisposedException(ToString());

            BulletAPI_BtDynamicsWorld_removeRigidBody(m_handle, body.Handle);
        }
示例#2
0
        public void removeRigidBody(btRigidBody body)
        {
            if (m_disposed)
            {
                throw new ObjectDisposedException(ToString());
            }

            BulletAPI_BtDynamicsWorld_removeRigidBody(m_handle, body.Handle);
        }
示例#3
0
 internal void AddRigidBody(btRigidBody Body)
 {
     m_world.addRigidBody(Body);
 }
示例#4
0
        public override void SetTerrain(float[] heightMap, double[,] normalHeightMap)
        {
            if (m_terrainShape != null)
                DeleteTerrain();

            float hfmax = -9000;
            float hfmin = 90000;
            
            for (int i = 0; i <heightMap.Length;i++)
            {
                if (Single.IsNaN(heightMap[i]) || Single.IsInfinity(heightMap[i]))
                {
                    heightMap[i] = 0f;
                }

                hfmin = (heightMap[i] < hfmin) ? heightMap[i] : hfmin;
                hfmax = (heightMap[i] > hfmax) ? heightMap[i] : hfmax;
            }
            // store this for later reference.
            // Note, we're storing it  after we check it for anomolies above
            _origheightmap = heightMap;

            hfmin = 0;
            hfmax = 256;

            m_terrainShape = new btHeightfieldTerrainShape((int)Constants.RegionSize, (int)Constants.RegionSize, heightMap,
                                                           1.0f, hfmin, hfmax, (int)btHeightfieldTerrainShape.UPAxis.Z,
                                                           (int)btHeightfieldTerrainShape.PHY_ScalarType.PHY_FLOAT, false);
            float AabbCenterX = Constants.RegionSize/2f;
            float AabbCenterY = Constants.RegionSize/2f;

            float AabbCenterZ = 0;
            float temphfmin, temphfmax;

            temphfmin = hfmin;
            temphfmax = hfmax;

            if (temphfmin < 0)
            {
                temphfmax = 0 - temphfmin;
                temphfmin = 0 - temphfmin;
            }
            else if (temphfmin > 0)
            {
                temphfmax = temphfmax + (0 - temphfmin);
                //temphfmin = temphfmin + (0 - temphfmin);
            }
            AabbCenterZ = temphfmax/2f;
            
            if (m_terrainPosition == null)
            {
                m_terrainPosition = new btVector3(AabbCenterX, AabbCenterY, AabbCenterZ);
            }
            else
            {
                try
                {
                    m_terrainPosition.setValue(AabbCenterX, AabbCenterY, AabbCenterZ);
                } 
                catch (ObjectDisposedException)
                {
                    m_terrainPosition = new btVector3(AabbCenterX, AabbCenterY, AabbCenterZ);
                }
            }
            if (m_terrainMotionState != null)
            {
                m_terrainMotionState.Dispose();
                m_terrainMotionState = null;
            }
            m_terrainTransform = new btTransform(QuatIdentity, m_terrainPosition);
            m_terrainMotionState = new btDefaultMotionState(m_terrainTransform);
            TerrainBody = new btRigidBody(0, m_terrainMotionState, m_terrainShape);
            TerrainBody.setUserPointer((IntPtr)0);
            m_world.addRigidBody(TerrainBody);
        }
示例#5
0
 public void RemoveCollisionObject(btRigidBody body)
 {
     m_world.removeCollisionObject(body);
 }
        public override void SetTerrain (ITerrainChannel channel, short[] shortheightMap)
        {
            if (m_terrainShape != null)
                DeleteTerrain();

            float hfmax = 256;
            float hfmin = 0;
            
            // store this for later reference.
            // Note, we're storing it  after we check it for anomolies above
            _origheightmap = shortheightMap;

            hfmin = 0;
            hfmax = 256;

            float[] heightmap = new float[m_region.RegionSizeX * m_region.RegionSizeX];
            for (int i = 0; i < shortheightMap.Length; i++)
            {
                heightmap[i] = shortheightMap[i] / Constants.TerrainCompression;
            }

            m_terrainShape = new btHeightfieldTerrainShape(m_region.RegionSizeX, m_region.RegionSizeY, heightmap,
                                                           1.0f, hfmin, hfmax, (int)btHeightfieldTerrainShape.UPAxis.Z,
                                                           (int)btHeightfieldTerrainShape.PHY_ScalarType.PHY_FLOAT, false);
            float AabbCenterX = m_region.RegionSizeX / 2f;
            float AabbCenterY = m_region.RegionSizeY / 2f;

            float AabbCenterZ = 0;
            float temphfmin, temphfmax;

            temphfmin = hfmin;
            temphfmax = hfmax;

            if (temphfmin < 0)
            {
                temphfmax = 0 - temphfmin;
                temphfmin = 0 - temphfmin;
            }
            else if (temphfmin > 0)
            {
                temphfmax = temphfmax + (0 - temphfmin);
                //temphfmin = temphfmin + (0 - temphfmin);
            }
            AabbCenterZ = temphfmax/2f;
            
            if (m_terrainPosition == null)
            {
                m_terrainPosition = new btVector3(AabbCenterX, AabbCenterY, AabbCenterZ);
            }
            else
            {
                try
                {
                    m_terrainPosition.setValue(AabbCenterX, AabbCenterY, AabbCenterZ);
                } 
                catch (ObjectDisposedException)
                {
                    m_terrainPosition = new btVector3(AabbCenterX, AabbCenterY, AabbCenterZ);
                }
            }
            if (m_terrainMotionState != null)
            {
                m_terrainMotionState.Dispose();
                m_terrainMotionState = null;
            }
            m_terrainTransform = new btTransform(QuatIdentity, m_terrainPosition);
            m_terrainMotionState = new btDefaultMotionState(m_terrainTransform);
            TerrainBody = new btRigidBody(0, m_terrainMotionState, m_terrainShape);
            TerrainBody.setUserPointer((IntPtr)0);
            m_world.addRigidBody(TerrainBody);
        }
示例#7
0
 public ClosestNotMeRayResultCallback(btRigidBody body)
 {
     m_handle = BulletHelper_CreateClosestNotMeRaycastResultCallback(body.Handle);
 }
        internal void Dispose()
        {
            m_parent_scene.RemoveAvatarFromList(this);
            if (Body != null)
            {
                if (Body.isInWorld())
                {
                    m_parent_scene.removeFromWorld(this, Body);
                    m_parent_scene.RemoveCollisionObject(Body);
                }
                Body.Dispose(); 
                Body = null;
            }

            if (m_aMotor != null)
            {
                if (m_aMotor.Handle != IntPtr.Zero)
                    m_parent_scene.getBulletWorld().removeConstraint(m_aMotor);

                m_aMotor.Dispose(); m_aMotor = null;
            }
            if (ClosestCastResult != null)
            {
                ClosestCastResult.Dispose(); ClosestCastResult = null;
            }
            if (Shell != null)
            {
                Shell.Dispose(); Shell = null;
            }
            tempQuat1.Dispose();
            tempTrans1.Dispose();
            tempVector1.Dispose();
            tempVector2.Dispose();
            tempVector3.Dispose();
            tempVector4.Dispose();
            tempVector5RayCast.Dispose();
            tempVector6RayCast.Dispose();

        }
示例#9
0
 public btGeneric6DofConstraint(btRigidBody rbA, btRigidBody rbB, btTransform frameInA, btTransform frameInB, bool useLinearReferenceFrameA)
     : base()
 {
     m_handle = BulletAPI_CreateBtGeneric6DofConstraint(rbA.Handle, rbB.Handle, frameInA.Handle, frameInB.Handle, useLinearReferenceFrameA);
 }
示例#10
0
        public void SetBody(float mass)
        {

            if (!IsPhysical || childrenPrim.Count == 0)
            {
                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);


                prim_geom.calculateLocalInertia(mass, tempInertia1);

                if (mass != 0)
                    _parent_scene.addActivePrim(this);
                else
                    _parent_scene.remActivePrim(this);

                //     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();
                }
                //Body.setCollisionFlags(Body.getCollisionFlags() | (int)ContactFlags.CF_CUSTOM_MATERIAL_CALLBACK);
                //Body.setUserPointer((IntPtr) (int)m_localID);
                _parent_scene.AddPrimToScene(this);
            }
            else
            {
                // bool hasTrimesh = false;
                lock (childrenPrim)
                {
                    foreach (BulletDotNETPrim chld in childrenPrim)
                    {
                        if (chld == null)
                            continue;

                        // if (chld.NeedsMeshing())
                        //     hasTrimesh = true;
                    }
                }

                //if (hasTrimesh)
                //{
                ProcessGeomCreationAsTriMesh(Vector3.Zero, Quaternion.Identity);
                // createmesh returns null when it doesn't mesh.

                /*
                if (_mesh is Mesh)
                {
                }
                else
                {
                    m_log.Warn("[PHYSICS]: Can't link a OpenSim.Region.Physics.Meshing.Mesh object");
                    return;
                }
                */



                foreach (BulletDotNETPrim chld in childrenPrim)
                {
                    if (chld == null)
                        continue;
                    Vector3 offset = chld.Position - Position;
                    Vector3 pos = new Vector3(offset.X, offset.Y, offset.Z);
                    pos *= Quaternion.Inverse(Orientation);
                    //pos *= Orientation;
                    offset = pos;
                    chld.ProcessGeomCreationAsTriMesh(offset, chld.Orientation);

                    _mesh.Append(chld._mesh);


                }
                setMesh(_parent_scene, _mesh);

                //}

                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);


                prim_geom.calculateLocalInertia(mass, tempInertia1);

                if (mass != 0)
                    _parent_scene.addActivePrim(this);
                else
                    _parent_scene.remActivePrim(this);

                //     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);

            }

            if (IsPhysical)
                changeAngularLock(0);
        }
示例#11
0
        public BulletDotNETPrim(String primName, BulletDotNETScene parent_scene, Vector3 pos, Vector3 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);


            AxisLockLinearLow = new btVector3(-1 * (int)Constants.RegionSize, -1 * (int)Constants.RegionSize, -1 * (int)Constants.RegionSize);
            int regionsize = (int)Constants.RegionSize;

            if (regionsize == 256)
                regionsize = 512;

            AxisLockLinearHigh = new btVector3((int)Constants.RegionSize, (int)Constants.RegionSize, (int)Constants.RegionSize);

            _target_velocity = Vector3.Zero;
            _velocity = Vector3.Zero;
            _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 = Vector3.Zero;
            m_rotationalVelocity = Vector3.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);

        }
        internal void Dispose()
        {
            if (Body.isInWorld())
                m_parent_scene.removeFromWorld(Body);

            if (m_aMotor.Handle != IntPtr.Zero)
                m_parent_scene.getBulletWorld().removeConstraint(m_aMotor);

            m_aMotor.Dispose(); m_aMotor = null;
            ClosestCastResult.Dispose(); ClosestCastResult = null;
            Body.Dispose(); Body = null;
            Shell.Dispose(); Shell = null;
            tempQuat1.Dispose();
            tempTrans1.Dispose();
            tempVector1.Dispose();
            tempVector2.Dispose();
            tempVector3.Dispose();
            tempVector4.Dispose();
            tempVector5RayCast.Dispose();
            tempVector6RayCast.Dispose();

        }
示例#13
0
 internal void Enable(btRigidBody pBody, BulletDotNETPrim prim)
 {
     m_prim = prim;
     m_body = pBody;
 }
示例#14
0
 internal void removeFromWorld(BulletDotNETCharacter chr, btRigidBody body)
 {
     lock (m_characters)
     {
         if (m_characters.Contains(chr))
         {
             m_world.removeRigidBody(body);
             m_characters.Remove(chr);
         }
     }
 }
示例#15
0
 private void DisposeOfBody()
 {
     if (Body != null)
     {
         if (Body.Handle != IntPtr.Zero)
         {
             DisableAxisMotor();
             _parent_scene.removeFromWorld(this, Body);
             Body.Dispose();
         }
         Body = null;
         // TODO: dispose parts that make up body
     }
 }
示例#16
0
        internal void removeFromWorld(BulletDotNETPrim prm ,btRigidBody body)
        {
            lock (m_prims)
            {
                if (m_prims.Contains(prm))
                {
                    m_world.removeRigidBody(body);
                }
                remActivePrim(prm);
                m_primsLocalID.Remove(prm.m_localID);
                m_prims.Remove(prm);
            }

        }
 public ClosestNotMeRayResultCallback(btRigidBody body)
 {
     m_handle = BulletHelper_CreateClosestNotMeRaycastResultCallback(body.Handle);
 }
示例#18
0
        /// <summary>
        /// This creates the Avatar's physical Surrogate at the position supplied
        /// </summary>
        /// <param name="npositionX"></param>
        /// <param name="npositionY"></param>
        /// <param name="npositionZ"></param>

        // WARNING: This MUST NOT be called outside of ProcessTaints, else we can have unsynchronized access
        // to ODE internals. ProcessTaints is called from within thread-locked Simulate(), so it is the only 
        // place that is safe to call this routine AvatarGeomAndBodyCreation.
        private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ)
        {
            
            if (CAPSULE_LENGTH <= 0)
            {
                m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid!  Setting it to the smallest possible size!");
                CAPSULE_LENGTH = 0.01f;

            }

            if (CAPSULE_RADIUS <= 0)
            {
                m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid!  Setting it to the smallest possible size!");
                CAPSULE_RADIUS = 0.01f;

            }

            Shell = new btCapsuleShape(CAPSULE_RADIUS, CAPSULE_LENGTH);

            if (m_bodyPosition == null)
                m_bodyPosition = new btVector3(npositionX, npositionY, npositionZ);

            m_bodyPosition.setValue(npositionX, npositionY, npositionZ);

            if (m_bodyOrientation == null)
                m_bodyOrientation = new btQuaternion(m_CapsuleOrientationAxis, (Utils.DEG_TO_RAD * 90));

            if (m_bodyTransform == null)
                m_bodyTransform = new btTransform(m_bodyOrientation, m_bodyPosition);
            else
            {
                m_bodyTransform.Dispose();
                m_bodyTransform = new btTransform(m_bodyOrientation, m_bodyPosition);
            }

            if (m_bodyMotionState == null)
                m_bodyMotionState = new btDefaultMotionState(m_bodyTransform);
            else
                m_bodyMotionState.setWorldTransform(m_bodyTransform);

            m_mass = Mass;

            Body = new btRigidBody(m_mass, m_bodyMotionState, Shell);
            // this is used for self identification. User localID instead of body handle
            Body.setUserPointer(new IntPtr((int)m_localID));
            
            if (ClosestCastResult != null)
                ClosestCastResult.Dispose();
            ClosestCastResult = new ClosestNotMeRayResultCallback(Body);

            m_parent_scene.AddRigidBody(Body);
            Body.setActivationState(4);
            if (m_aMotor != null)
            {
                if (m_aMotor.Handle != IntPtr.Zero)
                {
                    m_parent_scene.getBulletWorld().removeConstraint(m_aMotor);
                    m_aMotor.Dispose();
                }
                m_aMotor = null;
            }

            m_aMotor = new btGeneric6DofConstraint(Body, m_parent_scene.TerrainBody,
                                                                         m_parent_scene.TransZero,
                                                                         m_parent_scene.TransZero, false);
            m_aMotor.setAngularLowerLimit(m_parent_scene.VectorZero);
            m_aMotor.setAngularUpperLimit(m_parent_scene.VectorZero);
            
           
        }
示例#19
0
 internal void removeFromWorld(btRigidBody body)
 {
     
     m_world.removeRigidBody(body);
 }
示例#20
0
 public btGeneric6DofConstraint(btRigidBody rbA, btRigidBody rbB, btTransform frameInA, btTransform frameInB, bool useLinearReferenceFrameA)
     : base()
 {
     m_handle = BulletAPI_CreateBtGeneric6DofConstraint(rbA.Handle, rbB.Handle, frameInA.Handle, frameInB.Handle, useLinearReferenceFrameA);
 }
示例#21
0
        static void Main(string[] args)
        {
            btVector3 testvec = new btVector3(-2, 1, 0);
            Console.WriteLine(String.Format("Original: {0}", testvec.testStr()));
            btVector3 testvec2 = testvec.absolute();
            Console.WriteLine(String.Format("absolute: {0}", testvec2.testStr()));
            Console.WriteLine(String.Format("angle:{0}", testvec.angle(testvec2)));
            Console.WriteLine(String.Format("closestAxis(orig):{0}", testvec.closestAxis()));
            btVector3 testvec3 = testvec.cross(testvec2);
            Console.WriteLine(String.Format("cross: {0}", testvec3.testStr()));
            Console.WriteLine(String.Format("distance: {0}", testvec.distance(testvec2)));
            Console.WriteLine(String.Format("distance2: {0}", testvec.distance2(testvec2)));
            Console.WriteLine(String.Format("dot: {0}",  testvec.dot(testvec2)));
            Console.WriteLine(String.Format("furthestAxis(orig): {0}", testvec.furthestAxis()));
            btVector3 testvec4 = testvec.normalized();
            Console.WriteLine(String.Format("normalized: {0}", testvec4.testStr()));
            testvec4.setInterpolate3(testvec, testvec2, 0.5f);
            Console.WriteLine(String.Format("interpolate3: {0}", testvec4.testStr()));
            testvec4.setValue(7f, -0.09f, 2.5f);
            Console.WriteLine(String.Format("setvec: {0}", testvec4.testStr()));
            testvec4.setX(5.0f);
            testvec4.setY(-0.25f);
            testvec4.setZ(90f);
            testvec.setValue(0, 0, -1024);
            testvec2.setValue(256, 256, 1024);
            Console.WriteLine(String.Format("setvecIndividual: {0}", testvec4.testStr()));
            btAxisSweep3 testbtAxisSweep3 = new btAxisSweep3(testvec, testvec2, 50);
            btDefaultCollisionConfiguration colconfig = new btDefaultCollisionConfiguration();
            btCollisionDispatcher coldisp = new btCollisionDispatcher(colconfig);
            btSequentialImpulseConstraintSolver seqimpconssol = new btSequentialImpulseConstraintSolver();
            btDiscreteDynamicsWorld dynamicsWorld = new btDiscreteDynamicsWorld(coldisp, testbtAxisSweep3, seqimpconssol,
                                                                                colconfig);
            dynamicsWorld.setGravity(new btVector3(0, 0, -9.87f));
            Console.WriteLine(String.Format("stepWorld: {0}", dynamicsWorld.stepSimulation((6f / 60), 5, (1f / 60))));
            Console.WriteLine(String.Format("stepWorld: {0}", dynamicsWorld.stepSimulation((6f / 60), 5, (1f / 60))));
            Console.WriteLine(String.Format("stepWorld: {0}", dynamicsWorld.stepSimulation((6f / 60), 5, (1f / 60))));
            Console.WriteLine(String.Format("stepWorld: {0}", dynamicsWorld.stepSimulation((6f / 60), 5, (1f / 60))));
            btQuaternion testquat = new btQuaternion(50, 0, 0, 1);
            btQuaternion testquatnorm = testquat.normalized();
            Console.WriteLine(String.Format("testquat: {0}", testquat.testStr()));
            Console.WriteLine(String.Format("testquatnormalize: {0}", testquatnorm.testStr()));
            Console.WriteLine(String.Format("testquatLength: {0}", testquat.length()));
            Console.WriteLine(String.Format("testquatnormalizeLength: {0}", testquatnorm.length()));

            float[] heightdata = new float[256*256];
            for (int j=0;j<256*256;j++)
            {
               if (j%2==0)
                    heightdata[j] = 21f;
               else
                   heightdata[j] = 28f;
                
            }

            btHeightfieldTerrainShape obj = new btHeightfieldTerrainShape(256, 256, heightdata, 1.0f, 0, 256,
                                                                          (int)btHeightfieldTerrainShape.UPAxis.Z,
                                                                          (int)btHeightfieldTerrainShape.PHY_ScalarType.
                                                                              PHY_FLOAT, false);

            btCapsuleShape cap = new btCapsuleShape(0.23f, 3);

            btTriangleMesh testMesh = new btTriangleMesh(true, false);
            testMesh.addTriangle(new btVector3(1, 0, 1), new btVector3(1, 0, -1), new btVector3(-1, 0, -1), false);
            testMesh.addTriangle(new btVector3(1, -1, 1), new btVector3(1, -1, -1), new btVector3(-1, -1, -1), false);
            testMesh.addTriangle(new btVector3(1, -1, 1), new btVector3(1, 0, 1), new btVector3(-1, -1, -1), false);
            testMesh.addTriangle(new btVector3(1, 0, 1), new btVector3(1, -1, -1), new btVector3(-1, 0, -1), false);
            testMesh.addTriangle(new btVector3(1, -1, -1), new btVector3(-1, 0, -1), new btVector3(-1, -1, -1), false);
            testMesh.addTriangle(new btVector3(1, -1, -1), new btVector3(1, 0, -1), new btVector3(-1, 0, -1), false);
            testMesh.addTriangle(new btVector3(1, 0, 1), new btVector3(1, -1, -1), new btVector3(1, 0, -1), false);
            testMesh.addTriangle(new btVector3(1, -1, 1), new btVector3(1, -1, -1), new btVector3(1, 0, 1), false);
            btGImpactMeshShape meshtest = new btGImpactMeshShape(testMesh);
            meshtest.updateBound();

            btRigidBody groundbody = new btRigidBody(0,
                                                     new btDefaultMotionState(
                                                         new btTransform(new btQuaternion(0, 0, 0, 1),
                                                                         new btVector3(128, 128, 256f / 2f))), obj,
                                                     new btVector3(0, 0, 0));

            btRigidBody capbody = new btRigidBody(200,
                                                     new btDefaultMotionState(
                                                         new btTransform(new btQuaternion(0, 0, 0, 1),
                                                                         new btVector3(128, 128, 25))), cap,
                                                     new btVector3(0, 0, 0));

            btRigidBody meshbody = new btRigidBody(200,
                                         new btDefaultMotionState(
                                             new btTransform(new btQuaternion(0, 0, 0, 1),
                                                             new btVector3(128, 128, 29))), meshtest,
                                         new btVector3(0, 0, 0));


            btRigidBodyConstructionInfo constructioninfotest = new btRigidBodyConstructionInfo();
            constructioninfotest.m_collisionShape = new btBoxShape(new btVector3(0.5f,0.5f,0.5f));
            constructioninfotest.m_localInertia = new btVector3(0, 0, 0);
            constructioninfotest.m_motionState = new btDefaultMotionState(new btTransform(new btQuaternion(0.3f, -0.4f, 0.8f, 0.1f), new btVector3(128.5f, 128, 25)),
                                                                          new btTransform(new btQuaternion(0,0,0,1),new btVector3(0,0.25f,0)));
            constructioninfotest.m_startWorldTransform = new btTransform(new btQuaternion(0,0,0,1),new btVector3(0,0,0));
            constructioninfotest.m_mass = 2000000;
            constructioninfotest.m_linearDamping = 0;
            constructioninfotest.m_angularDamping = 0;
            constructioninfotest.m_friction = 0.1f;
            constructioninfotest.m_restitution = 0;
            constructioninfotest.m_linearSleepingThreshold = 0.8f;
            constructioninfotest.m_angularSleepingThreshold = 1;
            constructioninfotest.m_additionalDamping = false;
            constructioninfotest.m_additionalDampingFactor = 0.005f;
            constructioninfotest.m_additionalLinearDampingThresholdSqr = 0.01f;
            constructioninfotest.m_additionalAngularDampingThresholdSqr = 0.01f;
            constructioninfotest.m_additionalAngularDampingFactor = 0.01f;
            constructioninfotest.commit();
            btGImpactCollisionAlgorithm.registerAlgorithm(coldisp);
            btRigidBody cubetest = new btRigidBody(constructioninfotest);

            dynamicsWorld.addRigidBody(groundbody);
            dynamicsWorld.addRigidBody(cubetest);
            dynamicsWorld.addRigidBody(capbody);
            dynamicsWorld.addRigidBody(meshbody);

            int frame = 0;
            for (int i = 0; i < 26; i++ )
            {
                int frames = dynamicsWorld.stepSimulation(((i%60) / 60f), 10, (1f / 60));
                frame += frames;
                Console.WriteLine(String.Format("Cube: frame {0} frames: {1} POS:{2}, quat:{3}", frame, frames, cubetest.getInterpolationWorldTransform().getOrigin().testStr(), cubetest.getWorldTransform().getRotation().testStr()));
                Console.WriteLine(String.Format("Cap: frame {0} frames: {1} POS:{2}, quat:{3}", frame, frames, capbody.getInterpolationWorldTransform().getOrigin().testStr(), capbody.getWorldTransform().getRotation().testStr()));
                Console.WriteLine(String.Format("Mesh: frame {0} frames: {1} POS:{2}, quat:{3}", frame, frames, meshbody.getInterpolationWorldTransform().getOrigin().testStr(), meshbody.getWorldTransform().getRotation().testStr()));
            
            }

            dynamicsWorld.removeRigidBody(meshbody);
            dynamicsWorld.removeRigidBody(capbody);
            dynamicsWorld.removeRigidBody(cubetest);
            dynamicsWorld.removeRigidBody(groundbody);
            cubetest.Dispose();
            groundbody.Dispose();
            capbody.Dispose();
            cap.Dispose();
            obj.Dispose();
            testbtAxisSweep3.Dispose();
            dynamicsWorld.Dispose();
            coldisp.Dispose();
            colconfig.Dispose();
            seqimpconssol.Dispose();


            testvec.Dispose();
            testvec2.Dispose();
            testvec3.Dispose();
            testvec4.Dispose();
            
        }