C# BulletDotNET.btQuaternion类代码示例

        public btQuaternion farthest(btQuaternion q)
        {
            if (m_disposed)
                throw new ObjectDisposedException(ToString());

            return FromIntPtr(BulletAPI_BtQuaternion_farthest(m_handle, q.Handle));
        }

        public float dot(btQuaternion q)
        {
            if (m_disposed)
                throw new ObjectDisposedException(ToString());

            return BulletAPI_BtQuaternion_dot(m_handle, q.Handle);
        }

        public BulletDotNETCharacter(string avName, BulletDotNETScene parent_scene, Vector3 pos, Vector3 size, float pid_d, float pid_p, float capsule_radius, float tensor, float density, float height_fudge_factor, float walk_divisor, float rundivisor)
        {
            m_position = pos;
            m_zeroPosition = pos;
            m_parent_scene = parent_scene;
            PID_D = pid_d;
            PID_P = pid_p;
            CAPSULE_RADIUS = capsule_radius;
            m_density = density;
            heightFudgeFactor = height_fudge_factor;
            walkDivisor = walk_divisor;
            runDivisor = rundivisor;
            
            for (int i = 0; i < 11; i++)
            {
                m_colliderarr[i] = false;
            }
            for (int i = 0; i < 11; i++)
            {
                m_colliderGroundarr[i] = false;
            }
            CAPSULE_LENGTH = (size.Z * 1.15f) - CAPSULE_RADIUS * 2.0f;
            m_tainted_CAPSULE_LENGTH = CAPSULE_LENGTH;
            m_isPhysical = false; // current status: no ODE information exists
            m_tainted_isPhysical = true; // new tainted status: need to create ODE information

            m_parent_scene.AddPhysicsActorTaint(this);
            
            // m_name = avName;
            tempVector1 = new btVector3(0, 0, 0);
            tempVector2 = new btVector3(0, 0, 0);
            tempVector3 = new btVector3(0, 0, 0);
            tempVector4 = new btVector3(0, 0, 0);

            tempVector5RayCast = new btVector3(0, 0, 0);
            tempVector6RayCast = new btVector3(0, 0, 0);
            tempVector7RayCast = new btVector3(0, 0, 0);

            tempQuat1 = new btQuaternion(0, 0, 0, 1);
            tempTrans1 = new btTransform(tempQuat1, tempVector1);
            // m_movementComparision = new PhysicsVector(0, 0, 0);
            m_CapsuleOrientationAxis = new btVector3(1, 0, 1);
        }

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

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

        private void rotate(float timestep)
        {
            m_log.Debug("[PHYSICS]: _________ChangeRotate");
            tempTransform2 = Body.getWorldTransform();
            tempOrientation2 = new btQuaternion(_orientation.X, _orientation.Y, _orientation.Z, _orientation.W);
            tempTransform2.setRotation(tempOrientation2);
            Body.setWorldTransform(tempTransform2);

            resetCollisionAccounting();
            m_taintrot = _orientation;
        }

        public btQuaternion slerp(btQuaternion q, float t)
        {
            if (m_disposed)
                throw new ObjectDisposedException(ToString());

            return FromIntPtr(BulletAPI_BtQuaternion_slerp(m_handle, q.Handle, t));
        }

 public btMatrix3x3(btQuaternion q)
 {
     m_handle = BulletAPI_CreateBtMatrix3x3Quaternion(q.Handle);
 }

        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();
//.........这里部分代码省略.........

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

        }

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

        private void LimitRotation(float timestep)
        {
            btQuaternion rot = m_body.getWorldTransform().getRotation();
            bool changed = false;
            if (m_RollreferenceFrame != Quaternion.Identity)
            {
                if (rot.getX() >= m_RollreferenceFrame.X)
                    rot = new btQuaternion(rot.getX() - (m_RollreferenceFrame.X / 2), rot.getY(), rot.getZ(), rot.getW());
                if (rot.getX() <= -m_RollreferenceFrame.X)
                    rot = new btQuaternion(rot.getX() + (m_RollreferenceFrame.X / 2), rot.getY(), rot.getZ(), rot.getW());

                if (rot.getY() >= m_RollreferenceFrame.Y)
                    rot = new btQuaternion(rot.getX(), rot.getY() - (m_RollreferenceFrame.Y / 2), rot.getZ(), rot.getW());
                if (rot.getY() <= -m_RollreferenceFrame.Y)
                    rot = new btQuaternion(rot.getX(), rot.getY() + (m_RollreferenceFrame.Y / 2), rot.getZ(), rot.getW());
                
                changed = true;
            }
            if ((m_flags & VehicleFlag.LOCK_ROTATION) != 0)
            {
                rot = new btQuaternion(0,0,rot.getZ(),rot.getW());
                changed = true;
            }

            if (changed)
            {
                btTransform trans = m_body.getWorldTransform();
                trans.setRotation(rot);
                m_body.setWorldTransform(trans);
            }
        }

 public btTransform(btQuaternion q, btVector3 v)
 {
     m_handle = BulletAPI_CreateBtTransformQuaternionVector3(q.Handle, v.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 void ProcessTaints(float timestep)
        {

            if (m_tainted_isPhysical != m_isPhysical)
            {
                if (m_tainted_isPhysical)
                {
                    // Create avatar capsule and related ODE data
                    if (!(Shell == null && Body == null))
                    {
                        m_log.Warn("[PHYSICS]: re-creating the following avatar ODE data, even though it already exists - "
                            + (Shell != null ? "Shell " : "")
                            + (Body != null ? "Body " : ""));
                    }
                    AvatarGeomAndBodyCreation(m_position.X, m_position.Y, m_position.Z);

                   
                }
                else
                {
                    // destroy avatar capsule and related ODE data

                    Dispose();
                    tempVector1 = new btVector3(0, 0, 0);
                    tempVector2 = new btVector3(0, 0, 0);
                    tempVector3 = new btVector3(0, 0, 0);
                    tempVector4 = new btVector3(0, 0, 0);

                    tempVector5RayCast = new btVector3(0, 0, 0);
                    tempVector6RayCast = new btVector3(0, 0, 0);
                    tempVector7RayCast = new btVector3(0, 0, 0);

                    tempQuat1 = new btQuaternion(0, 0, 0, 1);
                    tempTrans1 = new btTransform(tempQuat1, tempVector1);
                    // m_movementComparision = new PhysicsVector(0, 0, 0);
                    m_CapsuleOrientationAxis = new btVector3(1, 0, 1);
                }

                m_isPhysical = m_tainted_isPhysical;
            }

            if (m_tainted_CAPSULE_LENGTH != CAPSULE_LENGTH)
            {
                if (Body != null)
                {

                    m_pidControllerActive = true;
                    // no lock needed on _parent_scene.OdeLock because we are called from within the thread lock in OdePlugin's simulate()
                    //d.JointDestroy(Amotor);
                    float prevCapsule = CAPSULE_LENGTH;
                    CAPSULE_LENGTH = m_tainted_CAPSULE_LENGTH;
                    //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
                    Dispose();

                    tempVector1 = new btVector3(0, 0, 0);
                    tempVector2 = new btVector3(0, 0, 0);
                    tempVector3 = new btVector3(0, 0, 0);
                    tempVector4 = new btVector3(0, 0, 0);

                    tempVector5RayCast = new btVector3(0, 0, 0);
                    tempVector6RayCast = new btVector3(0, 0, 0);
                    tempVector7RayCast = new btVector3(0, 0, 0);

                    tempQuat1 = new btQuaternion(0, 0, 0, 1);
                    tempTrans1 = new btTransform(tempQuat1, tempVector1);
                    // m_movementComparision = new PhysicsVector(0, 0, 0);
                    m_CapsuleOrientationAxis = new btVector3(1, 0, 1);

                    AvatarGeomAndBodyCreation(m_position.X, m_position.Y,
                                      m_position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2));
                    Velocity = Vector3.Zero;

                }
                else
                {
                    m_log.Warn("[PHYSICS]: trying to change capsule size, but the following ODE data is missing - "
                        + (Shell == null ? "Shell " : "")
                        + (Body == null ? "Body " : ""));
                }
            }
            if (m_taintRemove)
            {
                Dispose();
            }
        }