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);
}
}
public CcdDemo()
{
NumObjects = 120;
Shapes[0] = new BoxShape(new Vector3(200, 10, 200));
for (int i = 0; i < NumObjects; i++)
{
CollisionShape shape = Shapes[ShapeIndex[i]];
shape.Margin = CollisionMargin;
bool isDynamic = i > 0;
Matrix trans = Matrix.Identity;
if (i > 0)
{
//stack them
int colsize = 10;
int row = (i * HalfExtents * 2) / (colsize * 2 * HalfExtents);
int row2 = row;
int col = (i) % (colsize) - colsize / 2;
if (col > 3)
{
col = 11;
row2 |= 1;
}
Vector3 pos;
if (NumObjects > 2)
{
pos = new Vector3(col * 2 * HalfExtents + (row2 % 2) * HalfExtents,
row * 2 * HalfExtents + HalfExtents + ExtraHeight, 0);
}
else
{
pos = new Vector3(0, -30, 0);
}
if (shape is BoxShape)
{
trans.Right = new Vector3(0, -1, 0);
trans.Up = new Vector3(1, 0, 0);
trans.Backward = new Vector3(0, 0, 1);
}
trans *= Matrix.CreateRotationZ(Microsoft.Xna.Framework.MathHelper.PiOver2);
trans.Translation = pos;
}
else
{
trans.Translation = new Vector3(0, -30, 0);
}
float mass = 1.0f;
if (!isDynamic)
mass = 0.0f;
RigidBody body = CreateRigidBody(mass, trans, shape);
body.CcdSquareMotionThreshold = HalfExtents;
body.CcdSweptSphereRadius = 0.2f * HalfExtents;
}
}
private void shootBox(Vector3 destination)
{
if (_world != null)
{
float mass = 1f;
Matrix startTransform = Matrix.Identity;
Vector3 camPos = _camera.Position;
startTransform.Translation = camPos;
//CollisionShape boxShape = new SphereShape(1);
CollisionShape boxShape = new BoxShape(Vector3.One);
RigidBody body = CreateRigidBody(mass, startTransform, boxShape);
Vector3 linVel = new Vector3(destination.X - camPos.X, destination.Y - camPos.Y, destination.Z - camPos.Z);
linVel.Normalize();
linVel *= _shootBoxInitialSpeed;
//body.getWorldTransform().setOrigin(camPos);
//body.getWorldTransform().setRotation(btQuaternion(0, 0, 0, 1));
body.LinearVelocity = linVel;
body.AngularVelocity = Vector3.Zero;
}
}
/// <summary>
/// Reinitializes physics.
/// </summary>
public void ResetPhysics()
{
_collisionDispatcher = new CollisionDispatcher();
if (_useSweepAndPrune)
{
Vector3 worldAabbMin = new Vector3(-10000, -10000, -10000);
Vector3 worldAabbMax = new Vector3(10000, 10000, 10000);
const int maxProxies = 32766;
_broadphase = new AxisSweep3(worldAabbMin, worldAabbMax, maxProxies);
}
else
_broadphase = new SimpleBroadphase();
_solver = new SequentialImpulseConstraintSolver();
_world = new DiscreteDynamicsWorld(_collisionDispatcher, _broadphase, _solver);
//world.setConstraintSolver(solver);
_world.Gravity = new Vector3(0, -10, 0);
_shapePtr = new CollisionShape[4];
_shapePtr[0] = new BoxShape(new Vector3(50, 10, 50));
_shapePtr[1] = new CylinderShape(new Vector3(_cubeHalfExtent - _collisionMargin, _cubeHalfExtent - _collisionMargin, _cubeHalfExtent - _collisionMargin));
_shapePtr[2] = new SphereShape(_cubeHalfExtent);
_shapePtr[3] = new BoxShape(new Vector3(_cubeHalfExtent, _cubeHalfExtent, _cubeHalfExtent));
_shapeIndex = new int[_maxNumObjects];
Matrix tr = Matrix.Identity;
for (int i = 0; i < _numObjects; i++)
{
if (i > 0)
// set shape
_shapeIndex[i] = 1;
else
_shapeIndex[i] = 0;
}
GC.Collect();
}
public ConstraintDemo()
{
CollisionShape shape = new BoxShape(new Vector3(HalfExtents, HalfExtents, HalfExtents));
Matrix trans = Matrix.Identity;
trans.Translation = new Vector3(0, 20, 0);
float mass = 1f;
//Point to Point constraint (ball socket)
{
RigidBody bodyA = CreateRigidBody(mass, trans, shape);
trans.Translation = new Vector3(2 * HalfExtents, 20, 0);
mass = 1f;
RigidBody bodyB = null;
//RigidBody bodyB = CreateRigidBody(mass, trans, shape);
//bodyB.ActivationState = ActivationState.DisableDeactivation;
//bodyB.SetDamping(0.3f, 0.3f);
Vector3 pivotInA = new Vector3(HalfExtents, -HalfExtents, -HalfExtents);
Vector3 axisInA = new Vector3(0, 0, 1);
Vector3 pivotInB = bodyB != null ? MathHelper.MatrixToVector(MathHelper.InvertMatrix(bodyB.CenterOfMassTransform), MathHelper.MatrixToVector(bodyA.CenterOfMassTransform, pivotInA)) : pivotInA;
Vector3 axisInB = bodyB != null ?
Vector3.TransformNormal(Vector3.TransformNormal(axisInA, bodyB.CenterOfMassTransform), MathHelper.InvertMatrix(bodyB.CenterOfMassTransform)) :
Vector3.TransformNormal(axisInA, bodyA.CenterOfMassTransform);
//TypedConstraint p2p = new Point2PointConstraint(bodyA, bodyB, pivotInA, pivotInB);
//TypedConstraint hinge = new HingeConstraint(bodyA, bodyB, pivotInA, pivotInB, axisInA, axisInB);
HingeConstraint hinge = new HingeConstraint(bodyA, pivotInA, axisInA);
//use zero targetVelocity and a small maxMotorImpulse to simulate joint friction
//float targetVelocity = 0.0f;
//float maxMotorImpulse = 0.01;
float targetVelocity = 1.0f;
float maxMotorImpulse = 1.0f;
hinge.EnableAngularMotor(true, targetVelocity, maxMotorImpulse);
PhysicsWorld.AddConstraint(hinge);
}
// create a slider, using the generic D6 constraint
{
mass = 1f;
Vector3 sliderWorldPos = new Vector3(0, 10, 0);
Vector3 sliderAxis = new Vector3(1, 0, 0);
float angle = 0;
Matrix sliderOrientation = Matrix.CreateFromQuaternion(new Quaternion(sliderAxis, angle));
trans = Matrix.Identity;
trans.Translation = sliderWorldPos;
//trans.setBasis(sliderOrientation);
_sliderTransform = trans;
_d6BodyA = CreateRigidBody(mass, trans, shape);
_d6BodyA.ActivationState = ActivationState.DisableDeactivation;
RigidBody fixedBody1 = CreateRigidBody(0, trans, null);
Matrix frameInA, frameInB;
frameInA = Matrix.Identity;
frameInB = Matrix.Identity;
Generic6DofConstraint slider = new Generic6DofConstraint(_d6BodyA, fixedBody1, frameInA, frameInB);
slider.SetLinearLowerLimit(_lowerSliderLimit);
slider.SetLinearUpperLimit(_hiSliderLimit);
//range should be small, otherwise singularities will 'explode' the constraint
slider.SetAngularLowerLimit(new Vector3(10, 0, 0));
slider.SetAngularUpperLimit(new Vector3(0, 0, 0));
PhysicsWorld.AddConstraint(slider);
}
}
public float GetSphereDistance(CollisionObject boxObject, out Vector3 pointOnBox, out Vector3 pointOnSphere, Vector3 sphereCenter, float radius)
{
pointOnBox = new Vector3();
pointOnSphere = new Vector3();
float margins;
Vector3[] bounds = new Vector3[2];
BoxShape boxShape = boxObject.CollisionShape as BoxShape;
bounds[0] = -boxShape.HalfExtents;
bounds[1] = boxShape.HalfExtents;
margins = boxShape.Margin; //also add sphereShape margin?
Matrix m44T = boxObject.WorldTransform;
Vector3[] boundsVec = new Vector3[2];
float penetration;
boundsVec[0] = bounds[0];
boundsVec[1] = bounds[1];
Vector3 marginsVec = new Vector3(margins, margins, margins);
// add margins
bounds[0] += marginsVec;
bounds[1] -= marginsVec;
/////////////////////////////////////////////////
Vector3 tmp, prel, normal, v3P;
Vector3[] n = new Vector3[6];
float sep = 10000000.0f, sepThis;
n[0] = new Vector3(-1.0f, 0.0f, 0.0f);
n[1] = new Vector3(0.0f, -1.0f, 0.0f);
n[2] = new Vector3(0.0f, 0.0f, -1.0f);
n[3] = new Vector3(1.0f, 0.0f, 0.0f);
n[4] = new Vector3(0.0f, 1.0f, 0.0f);
n[5] = new Vector3(0.0f, 0.0f, 1.0f);
// convert point in local space
prel = MathHelper.InvXForm(m44T, sphereCenter);
bool found = false;
v3P = prel;
for (int i = 0; i < 6; i++)
{
int j = i < 3 ? 0 : 1;
if ((sepThis = (Vector3.Dot(v3P - bounds[j], n[i]))) > 0.0f)
{
v3P = v3P - n[i] * sepThis;
found = true;
}
}
//
if (found)
{
bounds[0] = boundsVec[0];
bounds[1] = boundsVec[1];
normal = Vector3.Normalize(prel - v3P);
pointOnBox = v3P + normal * margins;
pointOnSphere = prel - normal * radius;
if ((Vector3.Dot(pointOnSphere - pointOnBox, normal)) > 0.0f)
{
return(1.0f);
}
// transform back in world space
tmp = MathHelper.MatrixToVector(m44T, pointOnBox);
pointOnBox = tmp;
tmp = MathHelper.MatrixToVector(m44T, pointOnSphere);
pointOnSphere = tmp;
float seps2 = (pointOnBox - pointOnSphere).LengthSquared();
//if this fails, fallback into deeper penetration case, below
if (seps2 > MathHelper.Epsilon)
{
sep = -(float)Math.Sqrt(seps2);
normal = (pointOnBox - pointOnSphere);
normal *= 1f / sep;
}
return(sep);
}
//////////////////////////////////////////////////
// Deep penetration case
penetration = GetSpherePenetration(boxObject, ref pointOnBox, ref pointOnSphere, sphereCenter, radius, bounds[0], bounds[1]);
bounds[0] = boundsVec[0];
bounds[1] = boundsVec[1];
if (penetration <= 0.0f)
{
return(penetration - margins);
}
else
{
return(1.0f);
}
}
