public virtual void Rotate(IndexedQuaternion iq)
{
IndexedMatrix im = IndexedMatrix.CreateFromQuaternion(iq);
im._origin = m_graphicsWorldTrans._origin;
SetWorldTransform(ref im);
}
public override void ClientMoveAndDisplay(GameTime gameTime)
{
// OPTIONAL: We simply move our ghost objects around (without rotating them.....)==============
if (m_ghostObject != null || m_pairCachingGhostObject != null)
{
rad += 0.005f; // Bad (depends on PC speed)
float sinRad = (float)Math.Sin(rad);
float cosRad = (float)Math.Cos(rad);
if (m_ghostObject != null)
{
IndexedMatrix im = IndexedMatrix.CreateFromQuaternion(quatDeg45Y);
im._origin = new IndexedVector3(15 * cosRad, 5, -15 * sinRad);
m_ghostObject.SetWorldTransform(ref im);
}
if (m_pairCachingGhostObject != null)
{
IndexedMatrix im = IndexedMatrix.CreateFromQuaternion(quatDeg45Y);
im._origin = new IndexedVector3(-15 * cosRad, 7, 15 * sinRad);
m_pairCachingGhostObject.SetWorldTransform(ref im);
}
}
base.ClientMoveAndDisplay(gameTime);
// NEW => Retrives the content from all ghost objects in the world and call ProcessObectsInsideGhostObjects(...) for each ====
if (m_dynamicsWorld != null)
{
ObjectArray <CollisionObject> objsInsidePairCachingGhostObject = new ObjectArray <CollisionObject>(); // We might want this to be a member variable...
ObjectArray <CollisionObject> pObjsInsideGhostObject = null; // We will store a reference of the current array in this pointer
ObjectArray <CollisionObject> objs = m_dynamicsWorld.GetCollisionObjectArray();
for (int i = 0, sz = objs.Count; i < sz; i++)
{
CollisionObject o = objs[i];
GhostObject go = GhostObject.Upcast(o);
if (go != null)
{
objsInsidePairCachingGhostObject.Resize(0);
PairCachingGhostObject pgo = go as PairCachingGhostObject; // No upcast functionality...
if (pgo != null)
{
GetCollidingObjectsInsidePairCachingGhostObject((DiscreteDynamicsWorld)m_dynamicsWorld, pgo, objsInsidePairCachingGhostObject);
pObjsInsideGhostObject = objsInsidePairCachingGhostObject;
}
else
{
pObjsInsideGhostObject = go.GetOverlappingPairs(); // It's better not to try and copy the whole array, but to keep a reference to it!
// Side Note: btAlignedObjectArray < btCollisionObject* > objs = go.getOverlappingPairs(); (at the moment) makes my program crash on my system...
// Nevermind, that was the wrong way of doing it: btAlignedObjectArray < btCollisionObject* >& objs = go.getOverlappingPairs(); is much better.
}
// Here pObjsInsideGhostObject should be valid.
ProcessObectsInsideGhostObjects(pObjsInsideGhostObject, pgo != null);
}
}
}
}
//
public float SignedDistance(ref IndexedVector3 position, float margin, ConvexShape shape0, ref IndexedMatrix wtrs0, ref GjkEpaSolver2Results results)
{
using (GjkEpaSolver2MinkowskiDiff shape = BulletGlobals.GjkEpaSolver2MinkowskiDiffPool.Get())
using (GJK gjk = BulletGlobals.GJKPool.Get())
{
SphereShape shape1 = BulletGlobals.SphereShapePool.Get();
shape1.Initialize(margin);
IndexedMatrix wtrs1 = IndexedMatrix.CreateFromQuaternion(IndexedQuaternion.Identity);
wtrs0._origin = position;
Initialize(shape0, ref wtrs0, shape1, ref wtrs1, ref results, shape, false);
gjk.Initialise();
IndexedVector3 guess = new IndexedVector3(1);
GJKStatus gjk_status = gjk.Evaluate(shape, ref guess);
if (gjk_status == GJKStatus.Valid)
{
IndexedVector3 w0 = IndexedVector3.Zero;
IndexedVector3 w1 = IndexedVector3.Zero;
for (int i = 0; i < gjk.m_simplex.rank; ++i)
{
float p = gjk.m_simplex.p[i];
w0 += shape.Support(ref gjk.m_simplex.c[i].d, 0) * p;
IndexedVector3 temp = -gjk.m_simplex.c[i].d;
w1 += shape.Support(ref temp, 1) * p;
}
results.witnesses0 = wtrs0 * w0;
results.witnesses1 = wtrs0 * w1;
IndexedVector3 delta = results.witnesses1 - results.witnesses0;
float margin2 = shape0.GetMarginNonVirtual() + shape1.GetMarginNonVirtual();
float length = delta.Length();
results.normal = delta / length;
results.witnesses0 += results.normal * margin2;
return(length - margin2);
}
else
{
if (gjk_status == GJKStatus.Inside)
{
if (Penetration(shape0, ref wtrs0, shape1, ref wtrs1, ref gjk.m_ray, ref results))
{
IndexedVector3 delta = results.witnesses0 - results.witnesses1;
float length = delta.Length();
if (length >= MathUtil.SIMD_EPSILON)
{
results.normal = delta / length;
}
return(-length);
}
}
}
BulletGlobals.SphereShapePool.Free(shape1);
}
return(MathUtil.SIMD_INFINITY);
}
public static void IntegrateTransform(ref IndexedMatrix curTrans,ref IndexedVector3 linvel,ref IndexedVector3 angvel,float timeStep,out IndexedMatrix predictedTransform)
{
predictedTransform = IndexedMatrix.CreateTranslation(curTrans._origin + linvel * timeStep);
// #define QUATERNION_DERIVATIVE
#if QUATERNION_DERIVATIVE
IndexedVector3 pos;
IndexedQuaternion predictedOrn;
IndexedVector3 scale;
curTrans.Decompose(ref scale, ref predictedOrn, ref pos);
predictedOrn += (angvel * predictedOrn) * (timeStep * .5f));
predictedOrn.Normalize();
#else
//Exponential map
//google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
IndexedVector3 axis;
float fAngle = angvel.Length();
//limit the angular motion
if (fAngle*timeStep > ANGULAR_MOTION_THRESHOLD)
{
fAngle = ANGULAR_MOTION_THRESHOLD / timeStep;
}
if ( fAngle < 0.001f )
{
// use Taylor's expansions of sync function
axis = angvel*( 0.5f*timeStep-(timeStep*timeStep*timeStep)*(0.020833333333f)*fAngle*fAngle );
}
else
{
// sync(fAngle) = sin(c*fAngle)/t
axis = angvel*( (float)Math.Sin(0.5f*fAngle*timeStep)/fAngle );
}
IndexedQuaternion dorn = new IndexedQuaternion(axis.X,axis.Y,axis.Z,(float)Math.Cos( fAngle*timeStep*.5f) );
IndexedQuaternion orn0 = curTrans.GetRotation();
IndexedQuaternion predictedOrn = dorn * orn0;
predictedOrn.Normalize();
#endif
IndexedMatrix newMatrix = IndexedMatrix.CreateFromQuaternion(predictedOrn);
predictedTransform._basis = newMatrix._basis;
}
// min_y = -1000, max_y = 10
public btRaycastBar(IDebugDraw debugDraw, float ray_length, float z, float min_y, float max_y, float minX, float maxX)
{
m_debugDraw = debugDraw;
frame_counter = 0;
ms = 0;
max_ms = 0;
min_ms = 9999;
sum_ms_samples = 0;
sum_ms = 0;
dx = 10.0f;
//min_x = -20;
//max_x = 20;
this.min_x = minX;
this.max_x = maxX;
float startX = (minX + maxX) / 2;
this.min_y = min_y;
this.max_y = max_y;
sign = 1.0f;
float dalpha = MathUtil.SIMD_2_PI / NUMRAYS_IN_BAR;
for (int i = 0; i < NUMRAYS_IN_BAR; i++)
{
float alpha = dalpha * i;
// rotate around by alpha degrees y
IndexedMatrix tr = IndexedMatrix.CreateFromQuaternion(new IndexedQuaternion(Vector3.Up, alpha));
direction[i] = new IndexedVector3(1.0f, 0.0f, 0.0f);
direction[i] = tr * direction[i];
direction[i] = direction[i] * ray_length;
source[i] = new IndexedVector3(startX, max_y, z);
dest[i] = source[i] + direction[i];
dest[i].Y = min_y;
normal[i] = new IndexedVector3(1.0f, 0.0f, 0.0f);
}
}
public virtual void ProcessTriangle(IndexedVector3[] triangle, int partId, int triangleIndex)
{
//skip self-collisions
if ((m_partIdA == partId) && (m_triangleIndexA == triangleIndex))
{
return;
}
//skip duplicates (disabled for now)
//if ((m_partIdA <= partId) && (m_triangleIndexA <= triangleIndex))
// return;
//search for shared vertices and edges
int numshared = 0;
int[] sharedVertsA = new int[] { -1, -1, -1 };
int[] sharedVertsB = new int[] { -1, -1, -1 };
///skip degenerate triangles
float crossBSqr = IndexedVector3.Cross((triangle[1] - triangle[0]), (triangle[2] - triangle[0])).LengthSquared();
if (crossBSqr < m_triangleInfoMap.m_equalVertexThreshold)
{
return;
}
float crossASqr = IndexedVector3.Cross((m_triangleVerticesA[1] - m_triangleVerticesA[0]), (m_triangleVerticesA[2] - m_triangleVerticesA[0])).LengthSquared();
///skip degenerate triangles
if (crossASqr < m_triangleInfoMap.m_equalVertexThreshold)
{
return;
}
#if false
printf("triangle A[0] = (%f,%f,%f)\ntriangle A[1] = (%f,%f,%f)\ntriangle A[2] = (%f,%f,%f)\n",
m_triangleVerticesA[0].GetX(), m_triangleVerticesA[0].GetY(), m_triangleVerticesA[0].GetZ(),
m_triangleVerticesA[1].GetX(), m_triangleVerticesA[1].GetY(), m_triangleVerticesA[1].GetZ(),
m_triangleVerticesA[2].GetX(), m_triangleVerticesA[2].GetY(), m_triangleVerticesA[2].GetZ());
printf("partId=%d, triangleIndex=%d\n", partId, triangleIndex);
printf("triangle B[0] = (%f,%f,%f)\ntriangle B[1] = (%f,%f,%f)\ntriangle B[2] = (%f,%f,%f)\n",
triangle[0].GetX(), triangle[0].GetY(), triangle[0].GetZ(),
triangle[1].GetX(), triangle[1].GetY(), triangle[1].GetZ(),
triangle[2].GetX(), triangle[2].GetY(), triangle[2].GetZ());
#endif
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
if ((m_triangleVerticesA[i] - triangle[j]).LengthSquared() < m_triangleInfoMap.m_equalVertexThreshold)
{
sharedVertsA[numshared] = i;
sharedVertsB[numshared] = j;
numshared++;
///degenerate case
if (numshared >= 3)
{
return;
}
}
}
///degenerate case
if (numshared >= 3)
{
return;
}
}
switch (numshared)
{
case 0:
{
break;
}
case 1:
{
//shared vertex
break;
}
case 2:
{
//shared edge
//we need to make sure the edge is in the order V2V0 and not V0V2 so that the signs are correct
if (sharedVertsA[0] == 0 && sharedVertsA[1] == 2)
{
sharedVertsA[0] = 2;
sharedVertsA[1] = 0;
int tmp = sharedVertsB[1];
sharedVertsB[1] = sharedVertsB[0];
sharedVertsB[0] = tmp;
}
int hash = GetHash(m_partIdA, m_triangleIndexA);
TriangleInfo info = null;
if (m_triangleInfoMap.ContainsKey(hash))
{
info = m_triangleInfoMap[hash];
}
else
{
info = new TriangleInfo();
m_triangleInfoMap[hash] = info;
}
int sumvertsA = sharedVertsA[0] + sharedVertsA[1];
int otherIndexA = 3 - sumvertsA;
IndexedVector3 edge = new IndexedVector3(m_triangleVerticesA[sharedVertsA[1]] - m_triangleVerticesA[sharedVertsA[0]]);
TriangleShape tA = new TriangleShape(m_triangleVerticesA[0], m_triangleVerticesA[1], m_triangleVerticesA[2]);
int otherIndexB = 3 - (sharedVertsB[0] + sharedVertsB[1]);
TriangleShape tB = new TriangleShape(triangle[sharedVertsB[1]], triangle[sharedVertsB[0]], triangle[otherIndexB]);
//btTriangleShape tB(triangle[0],triangle[1],triangle[2]);
IndexedVector3 normalA;
IndexedVector3 normalB;
tA.CalcNormal(out normalA);
tB.CalcNormal(out normalB);
edge.Normalize();
IndexedVector3 edgeCrossA = IndexedVector3.Normalize(IndexedVector3.Cross(edge, normalA));
{
IndexedVector3 tmp = m_triangleVerticesA[otherIndexA] - m_triangleVerticesA[sharedVertsA[0]];
if (IndexedVector3.Dot(edgeCrossA, tmp) < 0)
{
edgeCrossA *= -1;
}
}
IndexedVector3 edgeCrossB = IndexedVector3.Cross(edge, normalB).Normalized();
{
IndexedVector3 tmp = triangle[otherIndexB] - triangle[sharedVertsB[0]];
if (IndexedVector3.Dot(edgeCrossB, tmp) < 0)
{
edgeCrossB *= -1;
}
}
float angle2 = 0;
float ang4 = 0.0f;
IndexedVector3 calculatedEdge = IndexedVector3.Cross(edgeCrossA, edgeCrossB);
float len2 = calculatedEdge.LengthSquared();
float correctedAngle = 0f;
IndexedVector3 calculatedNormalB = normalA;
bool isConvex = false;
if (len2 < m_triangleInfoMap.m_planarEpsilon)
{
angle2 = 0.0f;
ang4 = 0.0f;
}
else
{
calculatedEdge.Normalize();
IndexedVector3 calculatedNormalA = IndexedVector3.Cross(calculatedEdge, edgeCrossA);
calculatedNormalA.Normalize();
angle2 = GetAngle(ref calculatedNormalA, ref edgeCrossA, ref edgeCrossB);
ang4 = MathUtil.SIMD_PI - angle2;
float dotA = IndexedVector3.Dot(normalA, edgeCrossB);
///@todo: check if we need some epsilon, due to floating point imprecision
isConvex = (dotA < 0f);
correctedAngle = isConvex ? ang4 : -ang4;
IndexedQuaternion orn2 = new IndexedQuaternion(calculatedEdge, -correctedAngle);
IndexedMatrix rotateMatrix = IndexedMatrix.CreateFromQuaternion(orn2);
calculatedNormalB = new IndexedBasisMatrix(orn2) * normalA;
}
//alternatively use
//IndexedVector3 calculatedNormalB2 = quatRotate(orn,normalA);
switch (sumvertsA)
{
case 1:
{
IndexedVector3 edge1 = m_triangleVerticesA[0] - m_triangleVerticesA[1];
IndexedQuaternion orn = new IndexedQuaternion(edge1, -correctedAngle);
IndexedVector3 computedNormalB = MathUtil.QuatRotate(orn, normalA);
float bla = IndexedVector3.Dot(computedNormalB, normalB);
if (bla < 0)
{
computedNormalB *= -1;
info.m_flags |= TriangleInfoMap.TRI_INFO_V0V1_SWAP_NORMALB;
}
#if DEBUG_INTERNAL_EDGE
if ((computedNormalB - normalB).Length() > 0.0001f)
{
System.Console.WriteLine("warning: normals not identical");
}
#endif//DEBUG_INTERNAL_EDGE
info.m_edgeV0V1Angle = -correctedAngle;
if (isConvex)
{
info.m_flags |= TriangleInfoMap.TRI_INFO_V0V1_CONVEX;
}
break;
}
case 2:
{
IndexedVector3 edge1 = m_triangleVerticesA[2] - m_triangleVerticesA[0];
IndexedQuaternion orn = new IndexedQuaternion(edge1, -correctedAngle);
IndexedVector3 computedNormalB = MathUtil.QuatRotate(orn, normalA);
if (IndexedVector3.Dot(computedNormalB, normalB) < 0)
{
computedNormalB *= -1;
info.m_flags |= TriangleInfoMap.TRI_INFO_V2V0_SWAP_NORMALB;
}
#if DEBUG_INTERNAL_EDGE
if ((computedNormalB - normalB).Length() > 0.0001)
{
System.Console.WriteLine("warning: normals not identical");
}
#endif //DEBUG_INTERNAL_EDGE
info.m_edgeV2V0Angle = -correctedAngle;
if (isConvex)
{
info.m_flags |= TriangleInfoMap.TRI_INFO_V2V0_CONVEX;
}
break;
}
case 3:
{
IndexedVector3 edge1 = m_triangleVerticesA[1] - m_triangleVerticesA[2];
IndexedQuaternion orn = new IndexedQuaternion(edge1, -correctedAngle);
IndexedVector3 computedNormalB = MathUtil.QuatRotate(orn, normalA);
if (IndexedVector3.Dot(computedNormalB, normalB) < 0)
{
info.m_flags |= TriangleInfoMap.TRI_INFO_V1V2_SWAP_NORMALB;
computedNormalB *= -1;
}
#if DEBUG_INTERNAL_EDGE
if ((computedNormalB - normalB).Length() > 0.0001)
{
System.Console.WriteLine("warning: normals not identical");
}
#endif //DEBUG_INTERNAL_EDGE
info.m_edgeV1V2Angle = -correctedAngle;
if (isConvex)
{
info.m_flags |= TriangleInfoMap.TRI_INFO_V1V2_CONVEX;
}
break;
}
}
break;
}
default:
{
// printf("warning: duplicate triangle\n");
break;
}
}
}
protected override void Initialize()
{
base.Initialize();
SetCameraDistance(50.0f);
///collision configuration contains default setup for memory, collision setup
m_collisionConfiguration = new DefaultCollisionConfiguration();
//m_collisionConfiguration.setConvexConvexMultipointIterations();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new CollisionDispatcher(m_collisionConfiguration);
m_broadphase = new DbvtBroadphase();
///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
m_constraintSolver = new SequentialImpulseConstraintSolver();
m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);
IndexedVector3 gravity = new IndexedVector3(0, -10, 0);
m_dynamicsWorld.SetGravity(ref gravity);
// NEW => btGhostPairCallback =================================
m_ghostPairCallback = new GhostPairCallback();
m_dynamicsWorld.GetBroadphase().GetOverlappingPairCache().SetInternalGhostPairCallback(m_ghostPairCallback); // Needed once to enable ghost objects inside Bullet
// NEW => btGhostObject =======================================
m_ghostObject = new GhostObject();
CollisionShape shape = new BoxShape(new IndexedVector3(5f)); // As far as I know only the world aabb of the shape will be used (i.e. a box always parallel to the x,y,z axis of variable size)
m_collisionShapes.Add(shape);
m_ghostObject.SetCollisionShape(shape);
m_ghostObject.SetCollisionFlags(CollisionFlags.CF_NO_CONTACT_RESPONSE); // We can choose to make it "solid" if we want...
m_dynamicsWorld.AddCollisionObject(m_ghostObject, CollisionFilterGroups.SensorTrigger, CollisionFilterGroups.AllFilter & ~CollisionFilterGroups.SensorTrigger);
//m_ghostObject.setWorldTransform(btTransform(btQuaternion::getIdentity(),btVector3(0,5,-15)));
IndexedMatrix im = IndexedMatrix.CreateFromQuaternion(quatDeg45Y);
im._origin = new IndexedVector3(0, 5, -15);
m_ghostObject.SetWorldTransform(im);
// NEW => btPairCachingGhostObject ============================
m_pairCachingGhostObject = new PairCachingGhostObject();
shape = new ConeShape(7.0f, 14.0f);
m_collisionShapes.Add(shape);
m_pairCachingGhostObject.SetCollisionShape(shape);
m_pairCachingGhostObject.SetCollisionFlags(CollisionFlags.CF_NO_CONTACT_RESPONSE); // We can choose to make it "solid" if we want...
m_dynamicsWorld.AddCollisionObject(m_pairCachingGhostObject, CollisionFilterGroups.SensorTrigger, CollisionFilterGroups.AllFilter & ~CollisionFilterGroups.SensorTrigger);
//m_pairCachingGhostObject.setWorldTransform(btTransform(btQuaternion::getIdentity(),btVector3(0,5,15)));
im._origin = new IndexedVector3(0, 7, 15);
m_pairCachingGhostObject.SetWorldTransform(im);
//=============================================================
///create a few basic rigid bodies
CollisionShape groundShape = new BoxShape(new IndexedVector3(50));
m_collisionShapes.Add(groundShape);
IndexedMatrix groundTransform = IndexedMatrix.Identity;
groundTransform._origin = new IndexedVector3(0, -50, 0);
float mass = 0.0f;
LocalCreateRigidBody(mass, groundTransform, groundShape);
// spawn some cubes (code pasted from appBasicDemo...)
if (true)
{
//create a few dynamic rigidbodies
CollisionShape colShape = new BoxShape(new IndexedVector3(SCALING, SCALING, SCALING));
//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
//CollisionShape colShape = new CylinderShape(new IndexedVector3(1f, 1, 1f));
m_collisionShapes.Add(colShape);
/// Create Dynamic Objects
IndexedMatrix startTransform = IndexedMatrix.Identity;
mass = 1f;
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = mass != 0f;
IndexedVector3 localInertia = IndexedVector3.Zero;
if (isDynamic)
{
colShape.CalculateLocalInertia(mass, out localInertia);
}
float start_x = START_POS_X - ARRAY_SIZE_X / 2;
float start_y = START_POS_Y;
float start_z = START_POS_Z - ARRAY_SIZE_Z / 2;
for (int k = 0; k < ARRAY_SIZE_Y; k++)
{
for (int i = 0; i < ARRAY_SIZE_X; i++)
{
for (int j = 0; j < ARRAY_SIZE_Z; j++)
{
startTransform._origin = (new IndexedVector3(2.0f * i + start_x, 20 + 2.0f * k + start_y, 2.0f * j + start_z) * SCALING);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
DefaultMotionState myMotionState = new DefaultMotionState(startTransform, IndexedMatrix.Identity);
RigidBodyConstructionInfo rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, colShape, localInertia);
RigidBody body = new RigidBody(rbInfo);
//body.setContactProcessingThreshold(colShape.getContactBreakingThreshold());
body.SetActivationState(ActivationState.ISLAND_SLEEPING);
m_dynamicsWorld.AddRigidBody(body);
body.SetActivationState(ActivationState.ISLAND_SLEEPING);
body.SetUserPointer(String.Format("Box X{0} Y{1} Z{2}", k, i, j));
}
}
}
}
ClientResetScene();
}