Пример #1
0
        public static BM.Matrix Cast(this Matrix4x4 mat)
        {
            var output = new BM.Matrix();

            *(Matrix4x4 *)&output = mat;
            return(output);
        }
Пример #2
0
        public static Matrix4x4 Cast(this BM.Matrix mat)
        {
            var output = new Matrix4x4();

            *(BM.Matrix *)&output = mat;
            return(output);
        }
        //Bullet wants me to fill in worldTrans
        //This is called by bullet once when rigid body is added to the the world
        //For kinematic rigid bodies it is called every simulation step
        //[MonoPInvokeCallback(typeof(GetTransformDelegate))]
        public override void GetWorldTransform(out BM.Matrix worldTrans)
        {
            _xx = transform.rotation.x * transform.rotation.x;
            _yy = transform.rotation.y * transform.rotation.y;
            _zz = transform.rotation.z * transform.rotation.z;
            _xy = transform.rotation.x * transform.rotation.y;
            _zw = transform.rotation.z * transform.rotation.w;
            _zx = transform.rotation.z * transform.rotation.x;
            _yw = transform.rotation.y * transform.rotation.w;
            _yz = transform.rotation.y * transform.rotation.z;
            _xw = transform.rotation.x * transform.rotation.w;

            matrix.M11 = 1.0f - (2.0f * (_yy + _zz));
            matrix.M12 = 2.0f * (_xy + _zw);
            matrix.M13 = 2.0f * (_zx - _yw);

            matrix.M21 = 2.0f * (_xy - _zw);
            matrix.M22 = 1.0f - (2.0f * (_zz + _xx));
            matrix.M23 = 2.0f * (_yz + _xw);

            matrix.M31 = 2.0f * (_zx + _yw);
            matrix.M32 = 2.0f * (_yz - _xw);
            matrix.M33 = 1.0f - (2.0f * (_yy + _xx));

            matrix.M41 = transform.position.x;
            matrix.M42 = transform.position.y;
            matrix.M43 = transform.position.z;

            worldTrans = matrix;
        }
 //Bullet wants me to fill in worldTrans
 //This is called by bullet once when rigid body is added to the the world
 //For kinematic rigid bodies it is called every simulation step
 //[MonoPInvokeCallback(typeof(GetTransformDelegate))]
 public override void GetWorldTransform(out BM.Matrix worldTrans)
 {
     //Matrix4x4 trans = transform.localToWorldMatrix;
     //worldTrans = trans.ToBullet();
     BulletSharp.Math.Quaternion q = transform.rotation.ToBullet();
     BulletSharp.Math.Matrix.RotationQuaternion(ref q, out worldTrans);
     worldTrans.Origin = transform.position.ToBullet();
 }
Пример #5
0
 public override void DrawSphere(float radius, ref BM.Matrix trans, ref BM.Vector3 color)
 {
     UnityEngine.Vector3    pos   = BSExtensionMethods2.ExtractTranslationFromMatrix(ref trans);
     UnityEngine.Quaternion rot   = BSExtensionMethods2.ExtractRotationFromMatrix(ref trans);
     UnityEngine.Vector3    scale = BSExtensionMethods2.ExtractScaleFromMatrix(ref trans);
     UnityEngine.Color      c     = new UnityEngine.Color(color.X, color.Y, color.Z);
     BUtility.DebugDrawSphere(pos, rot, scale, UnityEngine.Vector3.one * radius, c);
 }
Пример #6
0
 public override void DrawPlane(ref BM.Vector3 planeNormal, float planeConst, ref BM.Matrix trans, ref BM.Vector3 color)
 {
     UnityEngine.Vector3    pos   = BSExtensionMethods2.ExtractTranslationFromMatrix(ref trans);
     UnityEngine.Quaternion rot   = BSExtensionMethods2.ExtractRotationFromMatrix(ref trans);
     UnityEngine.Vector3    scale = BSExtensionMethods2.ExtractScaleFromMatrix(ref trans);
     UnityEngine.Color      c     = new UnityEngine.Color(color.X, color.Y, color.Z);
     BUtility.DebugDrawPlane(pos, rot, scale, planeNormal.ToUnity(), planeConst, c);
 }
Пример #7
0
 public override void DrawCapsule(float radius, float halfHeight, int upAxis, ref BM.Matrix trans, ref BM.Vector3 color)
 {
     UnityEngine.Vector3    pos   = BSExtensionMethods2.ExtractTranslationFromMatrix(ref trans);
     UnityEngine.Quaternion rot   = BSExtensionMethods2.ExtractRotationFromMatrix(ref trans);
     UnityEngine.Vector3    scale = BSExtensionMethods2.ExtractScaleFromMatrix(ref trans);
     UnityEngine.Color      c     = new UnityEngine.Color(color.X, color.Y, color.Z);
     BUtility.DebugDrawCapsule(pos, rot, scale, radius, halfHeight, upAxis, c);
 }
Пример #8
0
        public override void DrawCapsule(float radius, float halfHeight, int upAxis, ref BM.Matrix trans, ref BM.Vector3 color)
        {
            var pos   = BulletExtensionMethods.ExtractTranslationFromMatrix(ref trans);
            var rot   = BulletExtensionMethods.ExtractRotationFromMatrix(ref trans);
            var scale = BulletExtensionMethods.ExtractScaleFromMatrix(ref trans);
            var c     = new Color(color.X, color.Y, color.Z);

            BUtility.DebugDrawCapsule(pos, rot, scale, radius, halfHeight, upAxis, c);
        }
Пример #9
0
        public override void DrawSphere(float radius, ref BM.Matrix trans, ref BM.Vector3 color)
        {
            var pos   = BulletExtensionMethods.ExtractTranslationFromMatrix(ref trans);
            var rot   = BulletExtensionMethods.ExtractRotationFromMatrix(ref trans);
            var scale = BulletExtensionMethods.ExtractScaleFromMatrix(ref trans);
            var c     = new Color(color.X, color.Y, color.Z);

            BUtility.DebugDrawSphere(pos, rot, scale, Vector3.one * radius, c);
        }
Пример #10
0
 public override void DrawBox(ref BM.Vector3 bbMin, ref BM.Vector3 bbMax, ref BM.Matrix trans, ref BM.Vector3 color)
 {
     UnityEngine.Vector3    pos   = BSExtensionMethods2.ExtractTranslationFromMatrix(ref trans);
     UnityEngine.Quaternion rot   = BSExtensionMethods2.ExtractRotationFromMatrix(ref trans);
     UnityEngine.Vector3    scale = BSExtensionMethods2.ExtractScaleFromMatrix(ref trans);
     UnityEngine.Vector3    size  = (bbMax - bbMin).ToUnity() / 2;
     UnityEngine.Color      c     = new UnityEngine.Color(color.X, color.Y, color.Z);
     BUtility.DebugDrawBox(pos, rot, scale, size, c);
 }
Пример #11
0
        public override void DrawPlane(ref BM.Vector3 planeNormal, float planeConst, ref BM.Matrix trans, ref BM.Vector3 color)
        {
            var pos   = BulletExtensionMethods.ExtractTranslationFromMatrix(ref trans);
            var rot   = BulletExtensionMethods.ExtractRotationFromMatrix(ref trans);
            var scale = BulletExtensionMethods.ExtractScaleFromMatrix(ref trans);
            var c     = new Color(color.X, color.Y, color.Z);

            BUtility.DebugDrawPlane(pos, rot, scale, planeNormal.ToUnity(), planeConst, c);
        }
Пример #12
0
        public override void DrawBox(ref BM.Vector3 bbMin, ref BM.Vector3 bbMax, ref BM.Matrix trans, ref BM.Vector3 color)
        {
            var pos   = BulletExtensionMethods.ExtractTranslationFromMatrix(ref trans);
            var rot   = BulletExtensionMethods.ExtractRotationFromMatrix(ref trans);
            var scale = BulletExtensionMethods.ExtractScaleFromMatrix(ref trans);
            var size  = (bbMax - bbMin).ToUnity();
            var c     = new Color(color.X, color.Y, color.Z);

            BUtility.DebugDrawBox(pos, rot, scale, size, c);
        }
Пример #13
0
        //Bullet calls this so I can copy bullet data to unity
        public override void SetWorldTransform(ref BM.Matrix m)
        {
            // gRally
            IsChanged   = true;
            NewPosition = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m);
            NewRotation = BSExtensionMethods2.ExtractRotationFromMatrix(ref m);
            // OPT NewScale = Native.UtoB(BSExtensionMethods2.ExtractScaleFromMatrix(ref m));

            //transform.position = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m);
            //transform.rotation = BSExtensionMethods2.ExtractRotationFromMatrix(ref m);
            //transform.localScale = BSExtensionMethods2.ExtractScaleFromMatrix(ref m);
        }
Пример #14
0
 public override void DrawTransform(ref BM.Matrix trans, float orthoLen)
 {
     UnityEngine.Vector3    pos = BSExtensionMethods2.ExtractTranslationFromMatrix(ref trans);
     UnityEngine.Quaternion rot = BSExtensionMethods2.ExtractRotationFromMatrix(ref trans);
     UnityEngine.Vector3    p1  = pos + rot * UnityEngine.Vector3.up * orthoLen;
     UnityEngine.Vector3    p2  = pos - rot * UnityEngine.Vector3.up * orthoLen;
     UnityEngine.Gizmos.DrawLine(p1, p2);
     p1 = pos + rot * UnityEngine.Vector3.right * orthoLen;
     p2 = pos - rot * UnityEngine.Vector3.right * orthoLen;
     UnityEngine.Gizmos.DrawLine(p1, p2);
     p1 = pos + rot * UnityEngine.Vector3.forward * orthoLen;
     p2 = pos - rot * UnityEngine.Vector3.forward * orthoLen;
     UnityEngine.Gizmos.DrawLine(p1, p2);
 }
Пример #15
0
        //Bullet calls this so I can copy bullet data to unity
        public override void SetWorldTransform(ref BM.Matrix m)
        {
            double timeStamp = -1;

            if (threadHelper != null)
            {
                timeStamp = threadHelper.TotalSimulationTime;
            }
            lock (lck)
            {
                previousBulletTransform = lastBulletTransform;
                lastBulletTransform     = new BulletTransformData(m, timeStamp);
                mustUpdateTransform     = true;
            }
        }
Пример #16
0
        public override void DrawTransform(ref BM.Matrix trans, float orthoLen)
        {
            var pos = BulletExtensionMethods.ExtractTranslationFromMatrix(ref trans);
            var rot = BulletExtensionMethods.ExtractRotationFromMatrix(ref trans);
            var p1  = pos + rot * Vector3.up * orthoLen;
            var p2  = pos - rot * Vector3.up * orthoLen;

            Gizmos.DrawLine(p1, p2);
            p1 = pos + rot * Vector3.right * orthoLen;
            p2 = pos - rot * Vector3.right * orthoLen;
            Gizmos.DrawLine(p1, p2);
            p1 = pos + rot * Vector3.forward * orthoLen;
            p2 = pos - rot * Vector3.forward * orthoLen;
            Gizmos.DrawLine(p1, p2);
        }
Пример #17
0
 /// <summary>
 /// 位置更新。获取bullet的位置信息,赋值给服务端及unity
 /// </summary>
 /// <param name="m"></param>
 //Bullet calls this so I can copy bullet data to unity
 public override void SetWorldTransform(ref BM.Matrix m)
 {
     i++;
     if (this.i == 1)
     {
         sw = new Stopwatch();
         sw.Start();
     }
     unit.Position   = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m);
     unit.Quaternion = BSExtensionMethods2.ExtractRotationFromMatrix(ref m);
     if (unit.Position.y < -5000)
     {
         sw.Stop();
         long xx = this.sw.ElapsedMilliseconds;
     }
 }
        public override void DrawTransform(ref BM.Matrix trans, float orthoLen)
        {
            Vector3    pos = BSExtensionMethods2.ExtractTranslationFromMatrix(ref trans);
            Quaternion rot = BSExtensionMethods2.ExtractRotationFromMatrix(ref trans);
            Vector3    p1  = pos + rot * Vector3.up * orthoLen;
            Vector3    p2  = pos - rot * Vector3.up * orthoLen;

            Gizmos.matrix = Matrix4x4.identity;
            Gizmos.DrawLine(p1, p2);
            p1 = pos + rot * Vector3.right * orthoLen;
            p2 = pos - rot * Vector3.right * orthoLen;
            Gizmos.DrawLine(p1, p2);
            p1 = pos + rot * Vector3.forward * orthoLen;
            p2 = pos - rot * Vector3.forward * orthoLen;
            Gizmos.DrawLine(p1, p2);
        }
Пример #19
0
 public Matrix(BsMatrix m)
 {
     this.M11 = m.M11;
     this.M12 = m.M12;
     this.M13 = m.M13;
     this.M14 = m.M14;
     this.M21 = m.M21;
     this.M22 = m.M22;
     this.M23 = m.M23;
     this.M24 = m.M24;
     this.M31 = m.M31;
     this.M32 = m.M32;
     this.M33 = m.M33;
     this.M34 = m.M34;
     this.M41 = m.M41;
     this.M42 = m.M42;
     this.M43 = m.M43;
     this.M44 = m.M44;
 }
Пример #20
0
 //Bullet wants me to fill in worldTrans
 //This is called by bullet once when rigid body is added to the the world
 //For kinematic rigid bodies it is called every simulation step
 //[MonoPInvokeCallback(typeof(GetTransformDelegate))]
 public override void GetWorldTransform(out BM.Matrix worldTrans)
 {
     BulletSharp.Math.Vector3    pos = unit.Position.ToBullet();
     BulletSharp.Math.Quaternion rot = unit.Quaternion.ToBullet();
     BulletSharp.Math.Matrix.AffineTransformation(1f, ref rot, ref pos, out worldTrans);
 }
Пример #21
0
        //called by Physics World just before constraint is added to world.
        //the current constraint properties are used to rebuild the constraint.
        internal override bool _BuildConstraint()
        {
            BPhysicsWorld world = BPhysicsWorld.Get();

            if (m_constraintPtr != null)
            {
                if (m_isInWorld && world != null)
                {
                    m_isInWorld = false;
                    world.RemoveConstraint(m_constraintPtr);
                }
            }
            BRigidBody targetRigidBodyA = GetComponent <BRigidBody>();

            if (targetRigidBodyA == null)
            {
                Debug.LogError("BSliderConstraint needs to be added to a component with a BRigidBody.");
                return(false);
            }
            if (!targetRigidBodyA.isInWorld)
            {
                world.AddRigidBody(targetRigidBodyA);
            }
            RigidBody rba = (RigidBody)targetRigidBodyA.GetCollisionObject();

            if (rba == null)
            {
                Debug.LogError("Constraint could not get bullet RigidBody from target rigid body");
                return(false);
            }
            if (m_constraintType == ConstraintType.constrainToAnotherBody)
            {
                if (m_otherRigidBody == null)
                {
                    Debug.LogError("Other rigid body was not set");
                    return(false);
                }
                if (!m_otherRigidBody.isInWorld)
                {
                    world.AddRigidBody(m_otherRigidBody);
                }
                RigidBody rbb = (RigidBody)m_otherRigidBody.GetCollisionObject();
                if (rbb == null)
                {
                    Debug.LogError("Constraint could not get bullet RigidBody from target rigid body");
                    return(false);
                }

                BM.Matrix frameInA, frameInOther;
                string    errormsg = "";
                if (CreateFramesA_B(m_localConstraintAxisX, m_localConstraintAxisY, m_localConstraintPoint, out frameInA, out frameInOther, ref errormsg))
                {
                    m_constraintPtr = new SliderConstraint(rbb, rba, frameInOther, frameInA, true);
                }
                else
                {
                    Debug.LogError(errormsg);
                    return(false);
                }
            }
            else
            {
                BulletSharp.Math.Matrix frameInA = BM.Matrix.Identity;
                string errormsg = "";
                if (CreateFrame(m_localConstraintAxisX, m_localConstraintAxisY, m_localConstraintPoint, ref frameInA, ref errormsg))
                {
                    m_constraintPtr = new SliderConstraint(rba, frameInA, true);
                }
                else
                {
                    Debug.LogError(errormsg);
                    return(false);
                }
            }
            SliderConstraint sl = (SliderConstraint)m_constraintPtr;

            sl.LowerLinearLimit = m_lowerLinearLimit;
            sl.UpperLinearLimit = m_upperLinearLimit;

            sl.LowerAngularLimit                     = m_lowerAngularLimitRadians;
            sl.UpperAngularLimit                     = m_upperAngularLimitRadians;
            m_constraintPtr.Userobject               = this;
            m_constraintPtr.DebugDrawSize            = m_debugDrawSize;
            m_constraintPtr.BreakingImpulseThreshold = m_breakingImpulseThreshold;
            return(true);
        }
 //Bullet calls this so I can copy bullet data to unity
 public override void SetWorldTransform(ref BM.Matrix m)
 {
     transform.position = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m, ref tempVector3);
     transform.rotation = BSExtensionMethods2.ExtractRotationFromMatrix(ref m, ref tempQuaternion);
 }
 //Bullet calls this so I can copy bullet data to unity
 public override void SetWorldTransform(ref BM.Matrix m)
 {
     transform.position = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m);
     transform.rotation = BSExtensionMethods2.ExtractRotationFromMatrix(ref m);
 }
Пример #24
0
        internal override bool _BuildConstraint()
        {
            BPhysicsWorld world = BPhysicsWorld.Get();

            if (m_constraintPtr != null && m_isInWorld && world != null)
            {
                m_isInWorld = false;
                world.RemoveConstraint(m_constraintPtr);
            }

            BRigidBody targetRigidBodyA = GetComponent <BRigidBody>();

            if (targetRigidBodyA == null)
            {
                Debug.LogError("Fixed Constraint needs to be added to a component with a BRigidBody.");
                return(false);
            }

            if (!targetRigidBodyA.isInWorld)
            {
                world.AddRigidBody(targetRigidBodyA);
            }

            RigidBody rba = (RigidBody)targetRigidBodyA.GetCollisionObject();

            if (rba == null)
            {
                Debug.LogError("Constraint could not get bullet RigidBody from target rigid body");
                return(false);
            }

            if (m_otherRigidBody == null)
            {
                Debug.LogError("Other rigid body is not set");
                return(false);
            }

            if (!m_otherRigidBody.isInWorld)
            {
                world.AddRigidBody(m_otherRigidBody);
            }

            RigidBody rbb = (RigidBody)m_otherRigidBody.GetCollisionObject();

            if (rbb == null)
            {
                Debug.LogError("Constraint could not get bullet RigidBody from target rigid body");
                return(false);
            }

            initialTransform = BM.Matrix.AffineTransformation(1f, localRotationOffset.ToBullet(), m_localConstraintPoint.ToBullet());

            m_constraintPtr = new FixedConstraint(rba, rbb, BM.Matrix.Identity, initialTransform)
            {
                Userobject                  = this,
                DebugDrawSize               = m_debugDrawSize,
                BreakingImpulseThreshold    = m_breakingImpulseThreshold,
                OverrideNumSolverIterations = m_overrideNumSolverIterations
            };

            return(true);
        }
Пример #25
0
 //Bullet wants me to fill in worldTrans
 //This is called by bullet once when rigid body is added to the the world
 //For kinematic rigid bodies it is called every simulation step
 //[MonoPInvokeCallback(typeof(GetTransformDelegate))]
 public override void GetWorldTransform(out BM.Matrix worldTrans)
 {
     BulletSharp.Math.Matrix.AffineTransformation(1f, ref rot, ref pos, out worldTrans);
 }
Пример #26
0
    //IMPORTANT Time.fixedTime must match the timestep being used here.
    public static List <UnityEngine.Vector3> SimulateBall(BRigidBody ballRb, UnityEngine.Vector3 ballThrowForce, int numberOfSimulationSteps, bool reverseOrder)
    {
        var ballPositions = new List <UnityEngine.Vector3>(numberOfSimulationSteps);

        //Create a World
        Debug.Log("Initialize physics");

        CollisionConfiguration CollisionConf;
        CollisionDispatcher    Dispatcher;
        BroadphaseInterface    Broadphase;
        CollisionWorld         cw;
        ConstraintSolver       Solver;

        BulletSharp.SoftBody.SoftBodyWorldInfo softBodyWorldInfo;

        //This should create a copy of the BPhysicsWorld with the same settings
        BPhysicsWorld bw = BPhysicsWorld.Get();

        bw.CreatePhysicsWorld(out cw, out CollisionConf, out Dispatcher, out Broadphase, out Solver, out softBodyWorldInfo);
        World = (DiscreteDynamicsWorld)cw;

        //Copy all existing rigidbodies in scene
        // IMPORTANT rigidbodies must be added to the offline world in the same order that they are in the source world
        // this is because collisions must be resolved in the same order for the sim to be deterministic
        DiscreteDynamicsWorld sourceWorld = (DiscreteDynamicsWorld)bw.world;

        BulletSharp.RigidBody   bulletBallRb = null;
        BulletSharp.Math.Matrix mm           = BulletSharp.Math.Matrix.Identity;
        for (int i = 0; i < sourceWorld.NumCollisionObjects; i++)
        {
            CollisionObject co = sourceWorld.CollisionObjectArray[i];
            if (co != null && co.UserObject is BRigidBody)
            {
                BRigidBody      rb            = (BRigidBody)co.UserObject;
                float           mass          = rb.isDynamic() ? rb.mass : 0f;
                BCollisionShape existingShape = rb.GetComponent <BCollisionShape>();
                CollisionShape  shape         = null;
                if (existingShape is BSphereShape)
                {
                    shape = ((BSphereShape)existingShape).CopyCollisionShape();
                }
                else if (existingShape is BBoxShape)
                {
                    shape = ((BBoxShape)existingShape).CopyCollisionShape();
                }

                RigidBody bulletRB = null;
                BulletSharp.Math.Vector3 localInertia = new BulletSharp.Math.Vector3();
                rb.CreateOrConfigureRigidBody(ref bulletRB, ref localInertia, shape, null);
                BulletSharp.Math.Vector3    pos = rb.GetCollisionObject().WorldTransform.Origin;
                BulletSharp.Math.Quaternion rot = rb.GetCollisionObject().WorldTransform.GetOrientation();
                BulletSharp.Math.Matrix.AffineTransformation(1f, ref rot, ref pos, out mm);
                bulletRB.WorldTransform = mm;
                World.AddRigidBody(bulletRB, rb.groupsIBelongTo, rb.collisionMask);
                if (rb == ballRb)
                {
                    bulletBallRb = bulletRB;
                    bulletRB.ApplyCentralImpulse(ballThrowForce.ToBullet());
                }
            }
        }

        //Step the simulation numberOfSimulationSteps times
        for (int i = 0; i < numberOfSimulationSteps; i++)
        {
            int numSteps = World.StepSimulation(1f / 60f, 10, 1f / 60f);
            ballPositions.Add(bulletBallRb.WorldTransform.Origin.ToUnity());
        }

        UnityEngine.Debug.Log("ExitPhysics");
        if (World != null)
        {
            //remove/dispose constraints
            int i;
            for (i = World.NumConstraints - 1; i >= 0; i--)
            {
                TypedConstraint constraint = World.GetConstraint(i);
                World.RemoveConstraint(constraint);
                constraint.Dispose();
            }

            //remove the rigidbodies from the dynamics world and delete them
            for (i = World.NumCollisionObjects - 1; i >= 0; i--)
            {
                CollisionObject obj  = World.CollisionObjectArray[i];
                RigidBody       body = obj as RigidBody;
                if (body != null && body.MotionState != null)
                {
                    body.MotionState.Dispose();
                }
                World.RemoveCollisionObject(obj);
                obj.Dispose();
            }

            World.Dispose();
            Broadphase.Dispose();
            Dispatcher.Dispose();
            CollisionConf.Dispose();
        }

        if (Broadphase != null)
        {
            Broadphase.Dispose();
        }
        if (Dispatcher != null)
        {
            Dispatcher.Dispose();
        }
        if (CollisionConf != null)
        {
            CollisionConf.Dispose();
        }

        return(ballPositions);
    }
        //called by Physics World just before constraint is added to world.
        //the current constraint properties are used to rebuild the constraint.
        internal override bool _BuildConstraint()
        {
            BPhysicsWorld world = BPhysicsWorld.Get();

            if (m_constraintPtr != null)
            {
                if (m_isInWorld && world != null)
                {
                    m_isInWorld = false;
                    world.RemoveConstraint(m_constraintPtr);
                }
            }
            BRigidBody targetRigidBodyA = GetComponent <BRigidBody>();

            if (targetRigidBodyA == null)
            {
                Debug.LogError("B6DOFConstraint needs to be added to a component with a BRigidBody.");
                return(false);
            }
            if (!targetRigidBodyA.isInWorld)
            {
                world.AddRigidBody(targetRigidBodyA);
            }

            RigidBody rba = (RigidBody)targetRigidBodyA.GetCollisionObject();

            if (rba == null)
            {
                Debug.LogError("Constraint could not get bullet RigidBody from target rigid body");
                return(false);
            }
            if (m_constraintType == ConstraintType.constrainToAnotherBody)
            {
                if (m_otherRigidBody == null)
                {
                    Debug.LogError("Other Rigid Body is not set.");
                    return(false);
                }
                if (!m_otherRigidBody.isInWorld)
                {
                    world.AddRigidBody(m_otherRigidBody);
                    return(false);
                }
                RigidBody rbb = (RigidBody)m_otherRigidBody.GetCollisionObject();
                if (rbb == null)
                {
                    Debug.LogError("Constraint could not get bullet RigidBody from target rigid body");
                    return(false);
                }

                BM.Matrix frameInA, frameInOther;
                string    errormsg = "";
                if (CreateFramesA_B(m_localConstraintAxisX, m_localConstraintAxisY, m_localConstraintPoint, out frameInA, out frameInOther, ref errormsg))
                {
                    m_constraintPtr = new Generic6DofConstraint(rbb, rba, frameInOther, frameInA, true);
                }
                else
                {
                    Debug.LogError(errormsg);
                    return(false);
                }
            }
            else
            {
                //TODO think about this
                BM.Matrix frameInA = BulletSharp.Math.Matrix.Identity;
                m_constraintPtr = new Generic6DofConstraint(rba, frameInA, false);
            }
            m_constraintPtr.Userobject = this;
            Generic6DofConstraint sl = (Generic6DofConstraint)m_constraintPtr;

            sl.LinearLowerLimit  = m_linearLimitLower.ToBullet();
            sl.LinearUpperLimit  = m_linearLimitUpper.ToBullet();
            sl.AngularLowerLimit = m_angularLimitLowerRadians.ToBullet();
            sl.AngularUpperLimit = m_angularLimitUpperRadians.ToBullet();
            sl.TranslationalLimitMotor.TargetVelocity = m_motorLinearTargetVelocity.ToBullet();
            sl.TranslationalLimitMotor.MaxMotorForce  = m_motorLinearMaxMotorForce.ToBullet();
            sl.BreakingImpulseThreshold = m_breakingImpulseThreshold;
            sl.DebugDrawSize            = m_debugDrawSize;
            return(true);
        }
Пример #28
0
        public bool CreateFrame(Vector3 forward, Vector3 up, Vector3 constraintPoint, ref BM.Matrix m, ref string errorMsg)
        {
            BM.Vector4 x;
            BM.Vector4 y;
            BM.Vector4 z;
            if (forward == Vector3.zero)
            {
                errorMsg = "forward vector must not be zero";
                return(false);
            }

            forward.Normalize();
            if (up == Vector3.zero)
            {
                errorMsg = "up vector must not be zero";
                return(false);
            }

            Vector3 right = Vector3.Cross(forward, up);

            if (right == Vector3.zero)
            {
                errorMsg = "forward and up vector must not be colinear";
                return(false);
            }

            up = Vector3.Cross(right, forward);
            right.Normalize();
            up.Normalize();
            x.X      = forward.x; x.Y = forward.y; x.Z = forward.z; x.W = 0f;
            y.X      = up.x; y.Y = up.y; y.Z = up.z; y.W = 0f;
            z.X      = right.x; z.Y = right.y; z.Z = right.z; z.W = 0f;
            m.Row1   = x;
            m.Row2   = y;
            m.Row3   = z;
            m.Origin = constraintPoint.ToBullet();

            return(true);
        }
Пример #29
0
        // Object A has the constraint compontent and is constrainted to object B
        // The constraint frame is defined relative to A by three vectors.
        // This method calculates the frames A and B that need to be passed to bullet
        public bool CreateFramesA_B(Vector3 forwardInA, Vector3 upInA, Vector3 constraintPivotInA, out BM.Matrix frameInA, out BM.Matrix frameInB, ref string errorMsg)
        {
            frameInA = BM.Matrix.Identity;
            if (!CreateFrame(forwardInA, upInA, constraintPivotInA, ref frameInA, ref errorMsg))
            {
                frameInB = BM.Matrix.Identity;
                return(false);
            }

            BM.Vector4 x  = frameInA.Row1;
            BM.Vector4 y  = frameInA.Row2;
            BM.Vector4 z  = frameInA.Row3;
            Vector3    xx = new Vector3(x.X, x.Y, x.Z);
            Vector3    yy = new Vector3(y.X, y.Y, y.Z);
            Vector3    zz = new Vector3(z.X, z.Y, z.Z);
            Quaternion q  = transform.localRotation * Quaternion.Inverse(m_otherRigidBody.transform.localRotation);

            xx              = q * xx;
            yy              = q * yy;
            zz              = q * zz;
            frameInB        = BM.Matrix.Identity;
            frameInB.Row1   = new BM.Vector4(xx.ToBullet(), 0f);
            frameInB.Row2   = new BM.Vector4(yy.ToBullet(), 0f);
            frameInB.Row3   = new BM.Vector4(zz.ToBullet(), 0f);
            frameInB.Origin = m_otherRigidBody.transform.InverseTransformPoint(transform.TransformPoint(constraintPivotInA)).ToBullet();

            return(true);
        }
        //called by Physics World just before constraint is added to world.
        //the current constraint properties are used to rebuild the constraint.
        internal override bool _BuildConstraint()
        {
            BPhysicsWorld world = BPhysicsWorld.Get();

            if (m_constraintPtr != null)
            {
                if (m_isInWorld && world != null)
                {
                    m_isInWorld = false;
                    world.RemoveConstraint(m_constraintPtr);
                    return(false);
                }
            }

            BRigidBody targetRigidBodyA = GetComponent <BRigidBody>();

            if (targetRigidBodyA == null)
            {
                Debug.LogError("ConeTwistConstraint needs to be added to a component with a BRigidBody.");
                return(false);
            }
            if (!targetRigidBodyA.isInWorld)
            {
                world.AddRigidBody(targetRigidBodyA);
            }
            if (m_constraintType == ConstraintType.constrainToAnotherBody)
            {
                if (m_otherRigidBody == null)
                {
                    Debug.LogError("Other rigid body was not set");
                    return(false);
                }
                if (!m_otherRigidBody.isInWorld)
                {
                    world.AddRigidBody(m_otherRigidBody);
                }
                BM.Matrix frameInA, frameInOther;
                string    errormsg = "";
                if (CreateFramesA_B(m_localConstraintAxisX, m_localConstraintAxisY, m_localConstraintPoint, out frameInA, out frameInOther, ref errormsg))
                {
                    m_constraintPtr = new ConeTwistConstraint((RigidBody)m_otherRigidBody.GetCollisionObject(), (RigidBody)targetRigidBodyA.GetCollisionObject(), frameInOther, frameInA);
                }
                else
                {
                    Debug.LogError(errormsg);
                    return(false);
                }
            }
            else
            {
                //TODO this is broken
                string    errormsg = "";
                BM.Matrix frameInB = BM.Matrix.Identity;
                if (CreateFrame(m_localConstraintAxisX, m_localConstraintAxisY, m_localConstraintPoint, ref frameInB, ref errormsg))
                {
                    m_constraintPtr = new ConeTwistConstraint((RigidBody)targetRigidBodyA.GetCollisionObject(), frameInB);
                }
                else
                {
                    Debug.LogError(errormsg);
                    return(false);
                }
            }
            ConeTwistConstraint sl = (ConeTwistConstraint)m_constraintPtr;

            sl.SetLimit(m_swingSpan1Radians, m_swingSpan2Radians, m_twistSpanRadians, m_softness, m_biasFactor, m_relaxationFactor);
            m_constraintPtr.Userobject               = this;
            m_constraintPtr.DebugDrawSize            = m_debugDrawSize;
            m_constraintPtr.BreakingImpulseThreshold = m_breakingImpulseThreshold;
            return(true);
        }