예제 #1
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();
        }
예제 #2
0
        private void MoveLinear(float timestep)
        {
            if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f))  // requested m_linearMotorDirection is significant
            {
                // add drive to body
                Vector3 addAmount = m_linearMotorDirection / (m_linearMotorTimescale / timestep);
                m_lastLinearVelocityVector += (addAmount * 10);  // lastLinearVelocityVector is the current body velocity vector?

                // This will work temporarily, but we really need to compare speed on an axis
                // KF: Limit body velocity to applied velocity?
                if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X))
                {
                    m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X;
                }
                if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y))
                {
                    m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y;
                }
                if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z))
                {
                    m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z;
                }

                // decay applied velocity
                Vector3 decayfraction = ((Vector3.One / (m_linearMotorDecayTimescale / timestep)));
                //Console.WriteLine("decay: " + decayfraction);
                m_linearMotorDirection -= m_linearMotorDirection * decayfraction * 0.5f;
                //Console.WriteLine("actual: " + m_linearMotorDirection);
            }
            else
            {        // requested is not significant
                // if what remains of applied is small, zero it.
                if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f))
                {
                    m_lastLinearVelocityVector = Vector3.Zero;
                }
            }

            // convert requested object velocity to world-referenced vector
            m_dir = m_lastLinearVelocityVector;
            btQuaternion rot  = m_body.getWorldTransform().getRotation();
            Quaternion   rotq = new Quaternion(rot.getX(), rot.getY(), rot.getZ(), rot.getW()); // rotq = rotation of object

            m_dir *= rotq;                                                                      // apply obj rotation to velocity vector

            // add Gravity andBuoyancy
            // KF: So far I have found no good method to combine a script-requested
            // .Z velocity and gravity. Therefore only 0g will used script-requested
            // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only.
            Vector3 grav = Vector3.Zero;
            // There is some gravity, make a gravity force vector
            // that is applied after object velocity.

            float objMass = m_prim.Mass;

            // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
            //Rev: bullet does gravity internally
            grav.Z = -parent_scene.gravityz * objMass * m_VehicleBuoyancy; //parent_scene.gravityz/* * objMass*/ * (1f - m_VehicleBuoyancy);

            // Preserve the current Z velocity
            btVector3 pos = m_body.getWorldTransform().getOrigin();

            btVector3 newpos = pos;

            m_dir.Z = m_prim.Velocity.Z; // Preserve the accumulated falling velocity

            Vector3 posChange = new Vector3();

            posChange.X = newpos.getX() - m_lastPositionVector.getX();
            posChange.Y = newpos.getY() - m_lastPositionVector.getY();
            posChange.Z = newpos.getZ() - m_lastPositionVector.getZ();
            btQuaternion Orientation2 = m_body.getWorldTransform().getRotation();

            /*if (m_BlockingEndPoint != Vector3.Zero)
             * {
             *  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;
        }