public void doForce(Vector3 force, bool now) { tempVector3.setValue(force.X, force.Y, force.Z); if (now) { Body.applyCentralForce(tempVector3); } else { m_taintedForce += force; m_parent_scene.AddPhysicsActorTaint(this); } }
public virtual void drawAabb( ref btVector3 from, ref btVector3 to, ref btVector3 color ) { btVector3 halfExtents; to.SubAndScale( ref from, 0.5f, out halfExtents ); btVector3 center; to.AddAndScale( ref from, 0.5f, out center ); int i, j; btVector3 edgecoord = new btVector3( 1f, 1f, 1f ), pa, pb; for( i = 0; i < 4; i++ ) { for( j = 0; j < 3; j++ ) { edgecoord.Mult( ref halfExtents, out pa ); pa.Add( ref center, out pa ); int othercoord = j % 3; edgecoord[othercoord] *= -1f; edgecoord.Mult( ref halfExtents, out pb ); pb.Add( ref center, out pb ); drawLine( ref pa, ref pb, ref color ); } edgecoord.setValue( -1, -1, -1 ); if( i < 3 ) edgecoord[i] *= -1; } }
public override void SetTerrain(float[] heightMap) { 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); }
/// <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 aurora.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 aurora.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(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 void CheckIfStandingOnObject() { float capsuleHalfHeight = ((CAPSULE_LENGTH + 2 * CAPSULE_RADIUS) * 0.5f); tempVector5RayCast.setValue(m_position.X, m_position.Y, m_position.Z); tempVector6RayCast.setValue(m_position.X, m_position.Y, m_position.Z - capsuleHalfHeight * 1.1f); ClosestCastResult.Dispose(); ClosestCastResult = new ClosestNotMeRayResultCallback(Body); try { m_parent_scene.getBulletWorld().rayTest(tempVector5RayCast, tempVector6RayCast, ClosestCastResult); } catch (AccessViolationException) { m_log.Debug("BAD!"); } if (ClosestCastResult.hasHit()) { if (tempVector7RayCast != null) { tempVector7RayCast.Dispose(); } //tempVector7RayCast = ClosestCastResult.getHitPointWorld(); /*if (tempVector7RayCast == null) // null == no result also * { * CollidingObj = false; * IsColliding = false; * CollidingGround = false; * * return; * } * float zVal = tempVector7RayCast.getZ(); * if (zVal != 0) * m_log.Debug("[PHYSICS]: HAAAA"); * if (zVal < m_position.Z && zVal > ((CAPSULE_LENGTH + 2 * CAPSULE_RADIUS) *0.5f)) * { * CollidingObj = true; * IsColliding = true; * } * else * { * CollidingObj = false; * IsColliding = false; * CollidingGround = false; * }*/ //height+2*radius = capsule full length //CollidingObj = true; //IsColliding = true; m_iscolliding = true; } else { //CollidingObj = false; //IsColliding = false; //CollidingGround = false; m_iscolliding = false; } }
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(); }
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; }
public override void calculateLocalInertia( double mass, out btVector3 inertia ) { //Until Bullet 2.77 a box approximation was used, so uncomment this if you need backwards compatibility //#define USE_BOX_INERTIA_APPROXIMATION 1 #if !USE_BOX_INERTIA_APPROXIMATION /* cylinder is defined as following: * * - principle axis aligned along y by default, radius in x, z-value not used * - for btCylinderShapeX: principle axis aligned along x, radius in y direction, z-value not used * - for btCylinderShapeZ: principle axis aligned along z, radius in x direction, y-value not used * */ double radius2; // square of cylinder radius double height2; // square of cylinder height btVector3 halfExtents; getHalfExtentsWithMargin( out halfExtents ); // get cylinder dimension double div12 = mass / 12.0f; double div4 = mass / 4.0f; double div2 = mass / 2.0f; int idxRadius, idxHeight; switch( m_upAxis ) // get indices of radius and height of cylinder { case 0: // cylinder is aligned along x idxRadius = 1; idxHeight = 0; break; case 2: // cylinder is aligned along z idxRadius = 0; idxHeight = 2; break; default: // cylinder is aligned along y idxRadius = 0; idxHeight = 1; break; } // calculate squares radius2 = halfExtents[idxRadius] * halfExtents[idxRadius]; height2 = (double)( 4.0 ) * halfExtents[idxHeight] * halfExtents[idxHeight]; // calculate tensor terms double t1 = div12 * height2 + div4 * radius2; double t2 = div2 * radius2; switch( m_upAxis ) // set diagonal elements of inertia tensor { case 0: // cylinder is aligned along x btVector3.setValue( out inertia, t2, t1, t1 ); break; case 2: // cylinder is aligned along z btVector3.setValue( out inertia, t1, t1, t2 ); break; default: // cylinder is aligned along y btVector3.setValue( out inertia, t1, t2, t1 ); break; } #else //USE_BOX_INERTIA_APPROXIMATION //approximation of box shape btVector3 halfExtents; getHalfExtentsWithMargin( out halfExtents ); double lx = (double)( 2.0) * ( halfExtents.x ); double ly = (double)( 2.0) * ( halfExtents.y ); double lz = (double)( 2.0) * ( halfExtents.z ); inertia.setValue( mass / ( (double)( 12.0 ) ) * ( ly * ly + lz * lz ), mass / ( (double)( 12.0 ) ) * ( lx * lx + lz * lz ), mass / ( (double)( 12.0 ) ) * ( lx * lx + ly * ly ) ); #endif //USE_BOX_INERTIA_APPROXIMATION }
///getAabb returns the axis aligned bounding box in the 'global' coordinate frame ///will add some transform later virtual void getBroadphaseAabb( ref btVector3 aabbMin, ref btVector3 aabbMax ) { aabbMin.setValue( -BT_LARGE_FLOAT, -BT_LARGE_FLOAT, -BT_LARGE_FLOAT ); aabbMax.setValue( BT_LARGE_FLOAT, BT_LARGE_FLOAT, BT_LARGE_FLOAT ); }
///computes the exact moment of inertia and the transform from the coordinate system defined by the principal axes of the moment of inertia ///and the center of mass to the current coordinate system. "masses" points to an array of masses of the children. The resulting transform ///"principal" has to be applied inversely to all children transforms in order for the local coordinate system of the compound ///shape to be centered at the center of mass and to coincide with the principal axes. This also necessitates a correction of the world transform ///of the collision object by the principal transform. void calculatePrincipalAxisTransform( double[] masses, ref btTransform principal, ref btVector3 inertia ) { int n = m_children.Count; double totalMass = 0; btVector3 center = btVector3.Zero; int k; for( k = 0; k < n; k++ ) { Debug.Assert( masses[k] > 0 ); center.AddScale( ref m_children[k].m_transform.m_origin, masses[k], out center ); totalMass += masses[k]; } Debug.Assert( totalMass > 0 ); center /= totalMass; principal.setOrigin( ref center ); btMatrix3x3 tensor = new btMatrix3x3( 0, 0, 0, 0, 0, 0, 0, 0, 0); for( k = 0; k < n; k++ ) { btVector3 i; m_children[k].m_childShape.calculateLocalInertia( masses[k], out i ); btTransform t = m_children[k].m_transform; btVector3 o; t.m_origin.Sub(ref center, out o ); //compute inertia tensor in coordinate system of compound shape btMatrix3x3 j; t.m_basis.transpose( out j); j.m_el0 *= i.x; j.m_el1 *= i.y; j.m_el2 *= i.z; btMatrix3x3 j2; t.m_basis.Mult( ref j, out j2 ); //add inertia tensor tensor.m_el0 += j2.m_el0; tensor.m_el1 += j2.m_el1; tensor.m_el2 += j2.m_el2; //compute inertia tensor of pointmass at o double o2 = o.length2(); j[0].setValue( o2, 0, 0 ); j[1].setValue( 0, o2, 0 ); j[2].setValue( 0, 0, o2 ); j.m_el0 += o * -o.x; j.m_el1 += o * -o.y; j.m_el2 += o * -o.z; //add inertia tensor of pointmass tensor.m_el0.AddScale( ref j.m_el0, masses[k], out tensor.m_el0 ); tensor.m_el1.AddScale( ref j.m_el1, masses[k], out tensor.m_el1 ); tensor.m_el2.AddScale( ref j.m_el2, masses[k], out tensor.m_el2 ); } tensor.diagonalize( out principal.m_basis, (double)( 0.00001 ), 20 ); inertia.setValue( tensor[0][0], tensor[1][1], tensor[2][2] ); }