//public AxisSweep3Internal(ref Vector3 worldAabbMin,ref Vector3 worldAabbMax, int handleMask, int handleSentinel, int maxHandles = 16384, OverlappingPairCache* pairCache=0,bool disableRaycastAccelerator = false);
public AxisSweep3Internal(ref Vector3 worldAabbMin, ref Vector3 worldAabbMax, int handleMask, ushort handleSentinel, ushort userMaxHandles, IOverlappingPairCache pairCache, bool disableRaycastAccelerator)
{
m_bpHandleMask = (handleMask);
m_handleSentinel = (handleSentinel);
m_pairCache = (pairCache);
m_userPairCallback = null;
m_ownsPairCache = (false);
m_invalidPair = 0;
m_raycastAccelerator = null;
ushort maxHandles = (ushort)(userMaxHandles+1);//need to add one sentinel handle
if (m_pairCache == null)
{
m_pairCache = new HashedOverlappingPairCache();
m_ownsPairCache = true;
}
if (!disableRaycastAccelerator)
{
m_nullPairCache = new NullPairCache();
m_raycastAccelerator = new DbvtBroadphase(m_nullPairCache);//m_pairCache);
m_raycastAccelerator.m_deferedcollide = true;//don't add/remove pairs
}
//btAssert(bounds.HasVolume());
// init bounds
m_worldAabbMin = worldAabbMin;
m_worldAabbMax = worldAabbMax;
Vector3 aabbSize = m_worldAabbMax - m_worldAabbMin;
int maxInt = m_handleSentinel;
m_quantize = new Vector3((float)maxInt,(float)maxInt,(float)maxInt) / aabbSize;
// allocate handles buffer, using btAlignedAlloc, and put all handles on free list
m_pHandles = new Handle[maxHandles];
for (int i = 0; i < m_pHandles.Length; ++i)
{
m_pHandles[i] = new Handle();
}
m_maxHandles = maxHandles;
m_numHandles = 0;
// handle 0 is reserved as the null index, and is also used as the sentinel
m_firstFreeHandle = 1;
{
for (ushort i = m_firstFreeHandle; i < maxHandles; i++)
{
ushort nextFree = (ushort)(i + (ushort)1);
m_pHandles[i].SetNextFree(nextFree);
}
m_pHandles[maxHandles - 1].SetNextFree(0);
}
{
m_pEdges = new Edge[3, (maxHandles * 2)];
// allocate edge buffers
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < maxHandles * 2; ++j)
{
m_pEdges[i,j] = new Edge();
}
}
}
//removed overlap management
// make boundary sentinels
m_pHandles[0].SetClientObject(null);
for (int axis = 0; axis < 3; axis++)
{
m_pHandles[0].m_minEdges[axis] = 0;
m_pHandles[0].m_maxEdges[axis] = 1;
m_pEdges[axis,0].m_pos = 0;
m_pEdges[axis,0].m_handle = 0;
m_pEdges[axis,1].m_pos = m_handleSentinel;
m_pEdges[axis,1].m_handle = 0;
#if DEBUG_BROADPHASE
debugPrintAxis(axis);
#endif //DEBUG_BROADPHASE
}
}
public virtual void Cleanup()
{
if (m_raycastAccelerator != null)
{
m_nullPairCache.Cleanup();
m_nullPairCache = null;
m_raycastAccelerator.Cleanup();
m_raycastAccelerator = null;
}
for (int i = 0; i < m_pHandles.Length; ++i)
{
m_pHandles[i].Cleanup();
}
m_pHandles = null;
if (m_ownsPairCache)
{
m_pairCache = null;
}
}
private static void Main(string[] args)
{
var collisionConfig = new DefaultCollisionConfiguration();
var dispatcher = new CollisionDispatcher(collisionConfig);
var pairCache = new DbvtBroadphase();
var solver = new SequentialImpulseConstraintSolver();
var dynamicsWorld = new DiscreteDynamicsWorld(dispatcher, pairCache, solver, collisionConfig);
dynamicsWorld.Gravity = new Vector3(0, -10, 0);
var groundShape = new BoxShape(new Vector3(50, 50, 50));
var groundTransform = Matrix.CreateTranslation(0, -56, 0);
{
float mass = 0;
bool isDynamic = mass != 0;
Vector3 localInertia = Vector3.Zero;
if (isDynamic)
localInertia = groundShape.CalculateLocalInertia(mass);
var motionState = new DefaultMotionState(groundTransform);
var rbinfo = new RigidBodyConstructionInfo(mass, motionState, groundShape, localInertia);
var body = new RigidBody(rbinfo);
dynamicsWorld.AddRigidBody(body);
}
{
var collisionShape = new SphereShape(1);
var transform = Matrix.Identity;
float mass = 1;
bool isDynamic = mass != 0;
Vector3 localInertia = Vector3.Zero;
if (isDynamic)
localInertia = collisionShape.CalculateLocalInertia(mass);
transform.Translation = new Vector3(2, 10, 0);
var motionState = new DefaultMotionState(transform);
var rbinfo = new RigidBodyConstructionInfo(mass, motionState, collisionShape, localInertia);
var body = new RigidBody(rbinfo);
dynamicsWorld.AddRigidBody(body);
}
Console.WriteLine("Starting simulation");
for (int i = 0; i < 100; i++)
{
dynamicsWorld.StepSimulation(1f / 60f, 10);
for (int j = dynamicsWorld.GetNumCollisionObjects() - 1; j >= 0; j--)
{
var obj = dynamicsWorld.GetCollisionObjectArray()[j];
var body = (RigidBody)obj;
if (body != null && body.GetMotionState() != null)
{
Matrix transform = Matrix.Identity;
body.GetMotionState().GetWorldTransform(ref transform);
Console.WriteLine("Object@{0} Position={1}", body.GetHashCode(), transform.Translation);
}
}
}
Console.Read();
}
