C# BulletDotNET.btTransform类代码示例

        public void getWorldTransform(out btTransform result)
        {
            if (m_disposed)
                throw new ObjectDisposedException(ToString());

            result = btTransform.getIdentity();
            BulletAPI_BtDefaultMotionState_getWorldTransform(m_handle, result.Handle);
        }

 public btDefaultMotionState(btTransform startTrans) : base()
 {
     m_handle = BulletAPI_CreateBtDefaultMotionStateStartTrans(startTrans.Handle);
 }

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

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

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

        public btTransform inverseTimes(btTransform t)
        {
            if (m_disposed)
                throw new ObjectDisposedException(ToString());

            return FromIntPtr(BulletAPI_BtTransform_inverseTimes(m_handle,t.Handle));
        }

 public btTransform(btTransform t)
 {
     m_handle = BulletAPI_CreateBtTransformOtherTransform(t.Handle);
 }

        public void mult(btTransform t1, btTransform t2)
        {
            if (m_disposed)
                throw new ObjectDisposedException(ToString());

            BulletAPI_BtTransform_mult(m_handle, t1.Handle, t2.Handle);
        }

 public void setWorldTransform(btTransform t)
 {
     if (m_disposed)
         throw new ObjectDisposedException(ToString());
     BulletAPI_BtDefaultMotionState_setWorldTransform(m_handle, t.Handle);
 }

//.........这里部分代码省略.........
                if (newpos.getX() >= (m_BlockingEndPoint.X - (float)1))
                    newpos.setX(newpos.getX() - (posChange.X + 1));
                if (newpos.getY() >= (m_BlockingEndPoint.Y - (float)1))
                    newpos.setY(newpos.getY() - (posChange.Y + 1));
                if (newpos.getZ() >= (m_BlockingEndPoint.Z - (float)1))
                    newpos.setZ(newpos.getZ() - (posChange.Z + 1));
                if (newpos.getX() <= 0)
                    newpos.setX(newpos.getX() + (posChange.X + 1));
                if (newpos.getY() <= 0)
                    newpos.setY(newpos.getY() + (posChange.Y + 1));
            }
            */
            if (newpos.getZ() < parent_scene.GetTerrainHeightAtXY(newpos.getX(), newpos.getY()))
                newpos.setZ(parent_scene.GetTerrainHeightAtXY(newpos.getX(), newpos.getY()) + 2);

            // Check if hovering
            if ((m_Hoverflags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
            {
                float diff = (newpos.getZ() - m_VhoverTargetHeight);
                // We should hover, get the target height
                if ((m_Hoverflags & VehicleFlag.HOVER_WATER_ONLY) != 0)
                    m_VhoverTargetHeight = parent_scene.GetWaterLevel() + m_VhoverHeight;
                if ((m_Hoverflags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
                    m_VhoverTargetHeight = parent_scene.GetTerrainHeightAtXY(pos.getX(), pos.getY()) + m_VhoverHeight;
                if ((m_Hoverflags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0)
                    m_VhoverTargetHeight = m_VhoverHeight;

                if ((m_Hoverflags & VehicleFlag.HOVER_UP_ONLY) != 0)
                {
                    // If body is aready heigher, use its height as target height
                    if (newpos.getZ() > m_VhoverTargetHeight) m_VhoverTargetHeight = newpos.getZ();
                }
                if ((m_Hoverflags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0)
                {
                    if (diff > .2 || diff < -.2)
                    {
                        newpos.setValue(newpos.getX(), newpos.getY(), m_VhoverTargetHeight);
                        btTransform trans = new btTransform(Orientation2, newpos);
                        m_body.setWorldTransform(trans);
                    }
                }
                else
                {
                    // Replace Vertical speed with correction figure if significant
                    if (Math.Abs(diff) > 0.01f)
                        m_dir.Z = -((diff * timestep * 50.0f) / m_VhoverTimescale);
                    else
                        m_dir.Z = 0f;
                }
            }

            /*if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0)
            {
                //Start Experimental Values
                if (Zchange > .3)
                    grav.Z = (float)(grav.Z * 3);
                if (Zchange > .15)
                    grav.Z = (float)(grav.Z * 2);
                if (Zchange > .75)
                    grav.Z = (float)(grav.Z * 1.5);
                if (Zchange > .05)
                    grav.Z = (float)(grav.Z * 1.25);
                if (Zchange > .025)
                    grav.Z = (float)(grav.Z * 1.125);
                
                float terraintemp = parent_scene.GetTerrainHeightAtXY(pos.getX(), pos.getY());
                float postemp = (pos.getZ() - terraintemp);
                
                if (postemp > 2.5f)
                    grav.Z = (float)(grav.Z * 1.037125);
                //End Experimental Values
            }*/

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

            m_lastPositionVector = new btVector3(m_prim.Position.X,m_prim.Position.Y,m_prim.Position.Z);
            // Apply velocity
            //if(m_dir != Vector3.Zero)
            //    m_body.setLinearVelocity(new btVector3(m_dir.X, m_dir.Y, m_dir.Z));
            m_body.applyCentralImpulse(new btVector3(m_dir.X, m_dir.Y, m_dir.Z));
            // apply gravity force
            //m_body.applyCentralImpulse(new btVector3(0, 0, 9.8f));

            /*ector3 newpos2 = new Vector3(newpos.getX(), newpos.getY(), newpos.getZ());
            if (newpos2.X != m_prim.Position.X || newpos2.Y != m_prim.Position.Y || newpos2.Z != m_prim.Position.Z)
            {
                btTransform trans = new btTransform(Orientation2, newpos);
                m_body.setWorldTransform(trans);
            }*/


            // apply friction
            Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / timestep);
            m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount;
        }

        private void changemove(float timestep)
        {

            m_log.Debug("[PHYSICS]: _________ChangeMove");
            if (!m_isphysical)
            {
                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();
            }
            else
            {
                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
                }
                /*
                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);
                }
                SetCollisionShape(prim_geom);
                */
                if (m_isphysical)
                    SetBody(Mass);
                else
                    SetBody(0);
                changeSelectedStatus(timestep);

                resetCollisionAccounting();
            }
            m_taintposition = _position;
        }

        public void setWorldTransform(btTransform worldTrans)
        {
            if (m_disposed)
                throw new ObjectDisposedException(ToString());

            BulletAPI_BtCollisionObject_setWorldTransform(m_handle, worldTrans.Handle);
        }

 public BulletDotNETScene(string sceneIdentifier)
 {
     BulletLock = new object();
     // m_sceneIdentifier = sceneIdentifier;
     VectorZero = new btVector3(0, 0, 0);
     QuatIdentity = new btQuaternion(0, 0, 0, 1);
     TransZero = new btTransform(QuatIdentity, VectorZero);
     m_gravity = new btVector3(0, 0, gravityz);
 }

 public BulletDotNETScene(string sceneIdentifier)
 {
     BulletLock = new object();
     // m_sceneIdentifier = sceneIdentifier;
     VectorZero = new btVector3(0, 0, 0);
     QuatIdentity = new btQuaternion(0, 0, 0, 1);
     TransZero = new btTransform(QuatIdentity, VectorZero);
     m_gravity = new btVector3(0, 0, gravityz);
     _origheightmap = new float[(int)Constants.RegionSize * (int)Constants.RegionSize];
 }

        private void changeshape(float timestep)
        {
            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
            }
            // 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

            ProcessGeomCreation();

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