public static OpenMetaverse.Vector3 XnaVector3ToPhysicsVector(Vector3 xnaVector3)
{
return new OpenMetaverse.Vector3(xnaVector3.X, xnaVector3.Y, xnaVector3.Z);
}
//added by jed zhu
private int CheckCollision(BulletXActor actA, int ia, int ib, int ic, Vector3 vNormal, Vector3[] vertBase)
{
Vector3 perPlaneNormal;
float fPerPlaneDist;
OpenMetaverse.Vector3 v = actA.Position;
Vector3 v3 = BulletXMaths.PhysicsVectorToXnaVector3(v);
//check AB
Vector3 v1;
v1 = vertBase[ib] - vertBase[ia];
Vector3.Cross(ref vNormal, ref v1, out perPlaneNormal);
Vector3.Normalize(ref perPlaneNormal, out perPlaneNormal);
if (Vector3.Dot((vertBase[ic] - vertBase[ia]), perPlaneNormal) < 0)
perPlaneNormal = -perPlaneNormal;
fPerPlaneDist = Vector3.Dot(perPlaneNormal, vertBase[ia]) - 0.50f;
if ((Vector3.Dot(perPlaneNormal, v3) - fPerPlaneDist) < 0)
return 0;
fPerPlaneDist = Vector3.Dot(perPlaneNormal, vertBase[ic]) + 0.50f;
if ((Vector3.Dot(perPlaneNormal, v3) - fPerPlaneDist) > 0)
return 0;
//check BC
v1 = vertBase[ic] - vertBase[ib];
Vector3.Cross(ref vNormal, ref v1, out perPlaneNormal);
Vector3.Normalize(ref perPlaneNormal, out perPlaneNormal);
if (Vector3.Dot((vertBase[ia] - vertBase[ib]), perPlaneNormal) < 0)
perPlaneNormal = -perPlaneNormal;
fPerPlaneDist = Vector3.Dot(perPlaneNormal, vertBase[ib]) - 0.50f;
if ((Vector3.Dot(perPlaneNormal, v3) - fPerPlaneDist) < 0)
return 0;
fPerPlaneDist = Vector3.Dot(perPlaneNormal, vertBase[ia]) + 0.50f;
if ((Vector3.Dot(perPlaneNormal, v3) - fPerPlaneDist) > 0)
return 0;
//check CA
v1 = vertBase[ia] - vertBase[ic];
Vector3.Cross(ref vNormal, ref v1, out perPlaneNormal);
Vector3.Normalize(ref perPlaneNormal, out perPlaneNormal);
if (Vector3.Dot((vertBase[ib] - vertBase[ic]), perPlaneNormal) < 0)
perPlaneNormal = -perPlaneNormal;
fPerPlaneDist = Vector3.Dot(perPlaneNormal, vertBase[ic]) - 0.50f;
if ((Vector3.Dot(perPlaneNormal, v3) - fPerPlaneDist) < 0)
return 0;
fPerPlaneDist = Vector3.Dot(perPlaneNormal, vertBase[ib]) + 0.50f;
if ((Vector3.Dot(perPlaneNormal, v3) - fPerPlaneDist) > 0)
return 0;
return 1;
}
public BulletXScene()
{
cDispatcher = new CollisionDispatcherLocal(this);
Vector3 worldMinDim = new Vector3((float) minXY, (float) minXY, (float) minZ);
Vector3 worldMaxDim = new Vector3((float) maxXY, (float) maxXY, (float) maxZ);
opCache = new AxisSweep3(worldMinDim, worldMaxDim, maxHandles);
sicSolver = new SequentialImpulseConstraintSolver();
lock (BulletXLock)
{
ddWorld = new DiscreteDynamicsWorld(cDispatcher, opCache, sicSolver);
ddWorld.Gravity = new Vector3(0, 0, -gravity);
}
//this._heightmap = new float[65536];
}
//added by jed zhu
//calculas the collision between the Prim and Actor
//
private int Collision(BulletXCharacter actorA, BulletXPrim primB)
{
int[] indexBase;
Vector3[] vertexBase;
Vector3 vNormal;
// Vector3 vP1;
// Vector3 vP2;
// Vector3 vP3;
IMesh mesh = primB.GetMesh();
float fdistance;
if (primB == null)
return 3;
if (mesh == null)
return 2;
if (actorA == null)
return 3;
int iVertexCount = mesh.getVertexList().Count;
int iIndexCount = mesh.getIndexListAsInt().Length;
if (iVertexCount == 0)
return 3;
if (iIndexCount == 0)
return 3;
lock (BulletXScene.BulletXLock)
{
indexBase = mesh.getIndexListAsInt();
vertexBase = new Vector3[iVertexCount];
for (int i = 0; i < iVertexCount; i++)
{
OpenMetaverse.Vector3 v = mesh.getVertexList()[i];
if (v != null) // Note, null has special meaning. See meshing code for details
vertexBase[i] = BulletXMaths.PhysicsVectorToXnaVector3(v);
else
vertexBase[i] = Vector3.Zero;
}
for (int ix = 0; ix < iIndexCount; ix += 3)
{
int ia = indexBase[ix + 0];
int ib = indexBase[ix + 1];
int ic = indexBase[ix + 2];
//
Vector3 v1 = vertexBase[ib] - vertexBase[ia];
Vector3 v2 = vertexBase[ic] - vertexBase[ia];
Vector3.Cross(ref v1, ref v2, out vNormal);
Vector3.Normalize(ref vNormal, out vNormal);
fdistance = Vector3.Dot(vNormal, vertexBase[ia]) + 0.50f;
if (preCheckCollision(actorA, vNormal, fdistance) == 1)
{
if (CheckCollision(actorA, ia, ib, ic, vNormal, vertexBase) == 1)
{
//PhysicsVector v = actorA.Position;
//Vector3 v3 = BulletXMaths.PhysicsVectorToXnaVector3(v);
//Vector3 vp = vNormal * (fdistance - Vector3.Dot(vNormal, v3) + 0.2f);
//actorA.Position += BulletXMaths.XnaVector3ToPhysicsVector(vp);
return 1;
}
}
}
}
return 0;
}
//added by jed zhu
//return value 1: need second check
//return value 0: no need check
private int preCheckCollision(BulletXActor actA, Vector3 vNormal, float fDist)
{
float fstartSide;
OpenMetaverse.Vector3 v = actA.Position;
Vector3 v3 = BulletXMaths.PhysicsVectorToXnaVector3(v);
fstartSide = Vector3.Dot(vNormal, v3) - fDist;
if (fstartSide > 0) return 0;
else return 1;
}
internal BulletXPlanet(BulletXScene parent_scene, float[] heightField)
{
_staticPosition = new OpenMetaverse.Vector3(BulletXScene.MaxXY / 2, BulletXScene.MaxXY / 2, 0);
// _staticVelocity = new PhysicsVector();
// _staticOrientation = OpenMetaverse.Quaternion.Identity;
_mass = 0; //No active
// _parentscene = parent_scene;
_heightField = heightField;
float _linearDamping = 0.0f;
float _angularDamping = 0.0f;
float _friction = 0.5f;
float _restitution = 0.0f;
Matrix _startTransform = Matrix.Identity;
Matrix _centerOfMassOffset = Matrix.Identity;
lock (BulletXScene.BulletXLock)
{
try
{
_startTransform.Translation = BulletXMaths.PhysicsVectorToXnaVector3(_staticPosition);
CollisionShape _collisionShape =
new HeightfieldTerrainShape(BulletXScene.MaxXY, BulletXScene.MaxXY, _heightField,
(float) BulletXScene.MaxZ, 2, true, false);
DefaultMotionState _motionState = new DefaultMotionState(_startTransform, _centerOfMassOffset);
Vector3 _localInertia = new Vector3();
//_collisionShape.CalculateLocalInertia(_mass, out _localInertia); //Always when mass > 0
_flatPlanet =
new RigidBody(_mass, _motionState, _collisionShape, _localInertia, _linearDamping,
_angularDamping, _friction, _restitution);
//It's seems that there are a bug with rigidBody constructor and its CenterOfMassPosition
Vector3 _vDebugTranslation;
_vDebugTranslation = _startTransform.Translation - _flatPlanet.CenterOfMassPosition;
_flatPlanet.Translate(_vDebugTranslation);
parent_scene.ddWorld.AddRigidBody(_flatPlanet);
}
catch (Exception ex)
{
BulletXScene.BulletXMessage(ex.Message, true);
}
}
BulletXScene.BulletXMessage("BulletXPlanet created.", false);
}
internal float HeightValue(Vector3 position)
{
int li_x, li_y;
float height;
li_x = (int) Math.Round(position.X);
if (li_x < 0) li_x = 0;
if (li_x >= BulletXScene.MaxXY) li_x = BulletXScene.MaxXY - 1;
li_y = (int) Math.Round(position.Y);
if (li_y < 0) li_y = 0;
if (li_y >= BulletXScene.MaxXY) li_y = BulletXScene.MaxXY - 1;
height = ((HeightfieldTerrainShape) _flatPlanet.CollisionShape).getHeightFieldValue(li_x, li_y);
if (height < 0) height = 0;
else if (height > BulletXScene.MaxZ) height = BulletXScene.MaxZ;
return height;
}
protected internal void CreateRigidBody(BulletXScene parent_scene, IMesh mesh, OpenMetaverse.Vector3 pos,
OpenMetaverse.Vector3 size)
{
//For RigidBody Constructor. The next values might change
float _linearDamping = 0.0f;
float _angularDamping = 0.0f;
float _friction = 1.0f;
float _restitution = 0.0f;
Matrix _startTransform = Matrix.Identity;
Matrix _centerOfMassOffset = Matrix.Identity;
//added by jed zhu
_mesh = mesh;
lock (BulletXScene.BulletXLock)
{
_startTransform.Translation = BulletXMaths.PhysicsVectorToXnaVector3(pos);
//For now all prims are boxes
CollisionShape _collisionShape;
if (mesh == null)
{
_collisionShape = new BoxShape(BulletXMaths.PhysicsVectorToXnaVector3(size)/2.0f);
}
else
{
int iVertexCount = mesh.getVertexList().Count;
int[] indices = mesh.getIndexListAsInt();
Vector3[] v3Vertices = new Vector3[iVertexCount];
for (int i = 0; i < iVertexCount; i++)
{
OpenMetaverse.Vector3 v = mesh.getVertexList()[i];
if (v != null) // Note, null has special meaning. See meshing code for details
v3Vertices[i] = BulletXMaths.PhysicsVectorToXnaVector3(v);
else
v3Vertices[i] = Vector3.Zero;
}
TriangleIndexVertexArray triMesh = new TriangleIndexVertexArray(indices, v3Vertices);
_collisionShape = new TriangleMeshShape(triMesh);
}
DefaultMotionState _motionState = new DefaultMotionState(_startTransform, _centerOfMassOffset);
Vector3 _localInertia = new Vector3();
if (_physical) _collisionShape.CalculateLocalInertia(Mass, out _localInertia); //Always when mass > 0
rigidBody =
new RigidBody(Mass, _motionState, _collisionShape, _localInertia, _linearDamping, _angularDamping,
_friction, _restitution);
//rigidBody.ActivationState = ActivationState.DisableDeactivation;
//It's seems that there are a bug with rigidBody constructor and its CenterOfMassPosition
Vector3 _vDebugTranslation;
_vDebugTranslation = _startTransform.Translation - rigidBody.CenterOfMassPosition;
rigidBody.Translate(_vDebugTranslation);
//---
parent_scene.ddWorld.AddRigidBody(rigidBody);
}
}
internal void Move(float timeStep)
{
Vector3 vec = new Vector3();
//At this point it's supossed that:
//_velocity == rigidBody.LinearVelocity
vec.X = _velocity.X;
vec.Y = _velocity.Y;
vec.Z = _velocity.Z;
if ((vec.X != 0.0f) || (vec.Y != 0.0f) || (vec.Z != 0.0f)) rigidBody.Activate();
if (flying)
{
//Antigravity with movement
if (_position.Z <= BulletXScene.HeightLevel0)
{
vec.Z += BulletXScene.Gravity*timeStep;
}
//Lowgravity with movement
else if ((_position.Z > BulletXScene.HeightLevel0)
&& (_position.Z <= BulletXScene.HeightLevel1))
{
vec.Z += BulletXScene.Gravity*timeStep*(1.0f - BulletXScene.LowGravityFactor);
}
//Lowgravity with...
else if (_position.Z > BulletXScene.HeightLevel1)
{
if (vec.Z > 0) //no movement
vec.Z = BulletXScene.Gravity*timeStep*(1.0f - BulletXScene.LowGravityFactor);
else
vec.Z += BulletXScene.Gravity*timeStep*(1.0f - BulletXScene.LowGravityFactor);
}
}
rigidBody.LinearVelocity = vec;
}
public BulletXCharacter(String avName, BulletXScene parent_scene, OpenMetaverse.Vector3 pos, OpenMetaverse.Vector3 velocity,
OpenMetaverse.Vector3 size, OpenMetaverse.Vector3 acceleration, OpenMetaverse.Quaternion orientation)
: base(avName)
{
//This fields will be removed. They're temporal
float _sizeX = 0.5f;
float _sizeY = 0.5f;
float _sizeZ = 1.6f;
//.
_position = pos;
_velocity = velocity;
_size = size;
//---
_size.X = _sizeX;
_size.Y = _sizeY;
_size.Z = _sizeZ;
//.
_acceleration = acceleration;
_orientation = orientation;
_physical = true;
float _mass = 50.0f; //This depends of avatar's dimensions
//For RigidBody Constructor. The next values might change
float _linearDamping = 0.0f;
float _angularDamping = 0.0f;
float _friction = 0.5f;
float _restitution = 0.0f;
Matrix _startTransform = Matrix.Identity;
Matrix _centerOfMassOffset = Matrix.Identity;
lock (BulletXScene.BulletXLock)
{
_startTransform.Translation = BulletXMaths.PhysicsVectorToXnaVector3(pos);
//CollisionShape _collisionShape = new BoxShape(new MonoXnaCompactMaths.Vector3(1.0f, 1.0f, 1.60f));
//For now, like ODE, collisionShape = sphere of radious = 1.0
CollisionShape _collisionShape = new SphereShape(1.0f);
DefaultMotionState _motionState = new DefaultMotionState(_startTransform, _centerOfMassOffset);
Vector3 _localInertia = new Vector3();
_collisionShape.CalculateLocalInertia(_mass, out _localInertia); //Always when mass > 0
rigidBody =
new RigidBody(_mass, _motionState, _collisionShape, _localInertia, _linearDamping, _angularDamping,
_friction, _restitution);
//rigidBody.ActivationState = ActivationState.DisableDeactivation;
//It's seems that there are a bug with rigidBody constructor and its CenterOfMassPosition
Vector3 _vDebugTranslation;
_vDebugTranslation = _startTransform.Translation - rigidBody.CenterOfMassPosition;
rigidBody.Translate(_vDebugTranslation);
parent_scene.ddWorld.AddRigidBody(rigidBody);
}
}
public override void ProcessAllTriangles(ITriangleCallback callback, MonoXnaCompactMaths.Vector3 aabbMin, MonoXnaCompactMaths.Vector3 aabbMax)
{
throw new Exception("The method or operation is not implemented.");
}
public static PhysicsVector XnaVector3ToPhysicsVector(Vector3 xnaVector3)
{
return new PhysicsVector(xnaVector3.X, xnaVector3.Y, xnaVector3.Z);
}