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