public void AddCollisionObject(CollisionObject collisionObject, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask)
{
//check that the object isn't already added
if (!_collisionObjects.Contains(collisionObject))
{
_collisionObjects.Add(collisionObject);
//calculate new AABB
Matrix trans = collisionObject.WorldTransform;
Vector3 minAabb;
Vector3 maxAabb;
collisionObject.CollisionShape.GetAabb(trans, out minAabb, out maxAabb);
BroadphaseNativeTypes type = collisionObject.CollisionShape.ShapeType;
collisionObject.Broadphase = Broadphase.CreateProxy(
minAabb,
maxAabb,
type,
collisionObject,
collisionFilterGroup,
collisionFilterMask
);
}
}
public void RemoveFromBroadphase(Body body)
{
foreach (var box in body.Boxes)
{
Broadphase.RemoveBox(box);
}
}
public void Update(double time)
{
//Maximize parallelism, divide narrow phase among threads
var broad_collisions = Broadphase.Solve(time);
//Solve narrow phase (subdivision intersections)
var final_collisions = Narrowphase.Solve(time, broad_collisions);
//Finally perform impulse resolution
}
public void InitTest()
{
Broadphase bf = new Broadphase();
bf.Init();
bf.Dispose();
}
public void RemoveBody(Body body)
{
Debug.Assert(body != null, "Yritetään poistaa null bodya!");
// todo hidas
if (colliders.Contains(body))
{
Broadphase.RemoveProxy(body.BroadphaseProxy);
colliders.Remove(body);
body.OnRemoved();
}
}
/// <summary>
/// Update loop
/// </summary>
/// <param name="gameTime">Kuinka paljon simuloidaan</param>
public void Step(GameTime gameTime)
{
// todo update velocity jne
// todo cache
for (int i = 0; i < colliders.Count; i++)
{
UpdateAABB(colliders[i]);
}
Broadphase.CalculateCollisionPairs();
Narrowphase.SolveCollisions(Broadphase.CollisionPairs);
}
void FixedUpdate()
{
Constants.SetConstants(compute, Time.fixedDeltaTime);
VelSimulation.Simulate();
Broadphase.FindBand(2f * constants.radius);
for (var i = 0; i < 4; i++)
{
ParticleSolver.Solve();
WallSolver.Solve();
Velocities.ClampMagnitude();
}
PosSimulation.Simulate();
BoundsChecker.Check();
Lifes.Simulate();
}
/// <summary>
/// Luo bodyn maailmaan ja alustaa broadphase proxyn
/// </summary>
/// <param name="body"></param>
/// <param name="group">Mihin collision ryhmään kuuluu</param>
/// <param name="mask">Minkä ryhmien kanssa törmää</param>
public void CreateBody(Body body, CollisionGroup group = CollisionGroup.Group1, CollisionGroup mask = CollisionGroup.All)
{
Debug.Assert(body != null, "Yritetään lisätä null bodya!");
// todo hidas
if (colliders.Contains(body))
{
return;
}
colliders.Add(body);
BroadphaseProxy proxy = BroadphaseProxy.Create(body, group, mask);
body.BroadphaseProxy = proxy;
Broadphase.AddProxy(ref proxy);
body.OnAdded();
}
public void RemoveCollisionObject(CollisionObject collisionObject)
{
BroadphaseProxy bp = collisionObject.Broadphase;
if (bp != null)
{
//
// only clear the cached algorithms
//
Broadphase.CleanProxyFromPairs(bp);
Broadphase.DestroyProxy(bp);
collisionObject.Broadphase = null;
}
_collisionObjects.Remove(collisionObject);
}
void OnDestroy()
{
if (Positions != null)
{
Positions.Dispose();
}
if (Velocities != null)
{
Velocities.Dispose();
}
if (Lifes != null)
{
Lifes.Dispose();
}
if (Constants != null)
{
Constants.Dispose();
}
if (Walls != null)
{
Walls.Dispose();
}
if (VelSimulation != null)
{
VelSimulation.Dispose();
}
if (PosSimulation != null)
{
PosSimulation.Dispose();
}
if (Broadphase != null)
{
Broadphase.Dispose();
}
if (WallSolver != null)
{
WallSolver.Dispose();
}
if (ParticleSolver != null)
{
ParticleSolver.Dispose();
}
if (Combiner != null)
{
Combiner.Dispose();
}
}
public void Dispose(bool disposing)
{
if (disposing)
{
//clean up remaining objects
foreach (CollisionObject collisionObject in _collisionObjects)
{
BroadphaseProxy bp = collisionObject.Broadphase;
if (bp != null)
{
//
// only clear the cached algorithms
//
Broadphase.CleanProxyFromPairs(bp);
Broadphase.DestroyProxy(bp);
}
}
}
}
private void Start()
{
switch (bType)
{
case broadphaseType.Dummy:
broadphase = Dummy.CreateInstance();
break;
case broadphaseType.AABBTree:
break;
}
switch (cType)
{
case collisionDetectionType.GJK:
collisionDetection = GJKwithEPA.CreateInstance();
break;
case collisionDetectionType.SAT:
break;
}
}
public void ExitPhysics()
{
// Remove/dispose constraints
for (int i = World.NumConstraints - 1; i >= 0; i--)
{
var constraint = World.GetConstraint(i);
World.RemoveConstraint(constraint);
constraint.Dispose();
}
// Remove/dispose rigid bodies
for (int i = World.NumCollisionObjects - 1; i >= 0; i--)
{
var obj = World.CollisionObjectArray[i];
var body = obj as RigidBody;
if (body != null && body.MotionState != null)
{
body.MotionState.Dispose();
}
World.RemoveCollisionObject(obj);
obj.Dispose();
}
//delete collision shapes
foreach (var shape in CollisionShapes)
{
shape.Dispose();
}
CollisionShapes.Clear();
World.Dispose();
Broadphase.Dispose();
if (Dispatcher != null)
{
Dispatcher.Dispose();
}
CollisionConf.Dispose();
}
/// <summary>
/// Päivittää GameObjectin bodyn BroadphaseProxyn AABB:n ajantasalle
/// </summary>
/// <param name="collider"></param>
private void UpdateAABB(Body collider)
{
AABB aabb = collider.GetAABB();
Broadphase.SetProxyAABB(collider.BroadphaseProxy, ref aabb);
}
public void RemoveBody(PhysicsBody body)
{
Broadphase.RemoveBody(body);
}
public void AddBody(PhysicsBody body)
{
Broadphase.AddBody(body);
}
public void Broadphase_InitializeTest()
{
var broadphase = new Broadphase(0, 0);
broadphase.Dispose();
}
public void AddForce(ForceField field)
{
Broadphase.AddBody(field.Area);
}
public override void ExitPhysics()
{
if (m_inverseModel != null)
{
Debug.Log("Dispose inverse model " + m_inverseModel.NumBodies);
m_inverseModel.Dispose();
}
Debug.Log("InverseDynamicsExitPhysics");
//cleanup in the reverse order of creation/initialization
//remove the rigidbodies from the dynamics world and delete them
if (m_dynamicsWorld == null)
{
int i;
for (i = m_dynamicsWorld.NumConstraints - 1; i >= 0; i--)
{
TypedConstraint tc = m_dynamicsWorld.GetConstraint(i);
m_dynamicsWorld.RemoveConstraint(tc);
tc.Dispose();
}
for (i = m_dynamicsWorld.NumMultiBodyConstraints - 1; i >= 0; i--)
{
MultiBodyConstraint mbc = m_dynamicsWorld.GetMultiBodyConstraint(i);
m_dynamicsWorld.RemoveMultiBodyConstraint(mbc);
mbc.Dispose();
}
for (i = m_dynamicsWorld.NumMultibodies - 1; i >= 0; i--)
{
MultiBody mb = m_dynamicsWorld.GetMultiBody(i);
m_dynamicsWorld.RemoveMultiBody(mb);
mb.Dispose();
}
for (i = m_dynamicsWorld.NumCollisionObjects - 1; i >= 0; i--)
{
CollisionObject obj = m_dynamicsWorld.CollisionObjectArray[i];
RigidBody body = RigidBody.Upcast(obj);
if (body != null && body.MotionState != null)
{
body.MotionState.Dispose();
}
m_dynamicsWorld.RemoveCollisionObject(obj);
obj.Dispose();
}
}
if (m_multiBody != null)
{
m_multiBody.Dispose();
}
//delete collision shapes
for (int j = 0; j < CollisionShapes.Count; j++)
{
CollisionShape shape = CollisionShapes[j];
shape.Dispose();
}
CollisionShapes.Clear();
m_dynamicsWorld.Dispose();
m_dynamicsWorld = null;
m_solver.Dispose();
m_solver = null;
Broadphase.Dispose();
Broadphase = null;
Dispatcher.Dispose();
Dispatcher = null;
m_pairCache.Dispose();
m_pairCache = null;
CollisionConf.Dispose();
CollisionConf = null;
Debug.Log("After dispose B");
}
//IMPORTANT Time.fixedTime must match the timestep being used here.
public static List <UnityEngine.Vector3> SimulateBall(BRigidBody ballRb, UnityEngine.Vector3 ballThrowForce, int numberOfSimulationSteps, bool reverseOrder)
{
var ballPositions = new List <UnityEngine.Vector3>(numberOfSimulationSteps);
//Create a World
Debug.Log("Initialize physics");
CollisionConfiguration CollisionConf;
CollisionDispatcher Dispatcher;
BroadphaseInterface Broadphase;
CollisionWorld cw;
ConstraintSolver Solver;
BulletSharp.SoftBody.SoftBodyWorldInfo softBodyWorldInfo;
//This should create a copy of the BPhysicsWorld with the same settings
BPhysicsWorld bw = BPhysicsWorld.Get();
bw.CreatePhysicsWorld(out cw, out CollisionConf, out Dispatcher, out Broadphase, out Solver, out softBodyWorldInfo);
World = (DiscreteDynamicsWorld)cw;
//Copy all existing rigidbodies in scene
// IMPORTANT rigidbodies must be added to the offline world in the same order that they are in the source world
// this is because collisions must be resolved in the same order for the sim to be deterministic
DiscreteDynamicsWorld sourceWorld = (DiscreteDynamicsWorld)bw.world;
BulletSharp.RigidBody bulletBallRb = null;
BulletSharp.Math.Matrix mm = BulletSharp.Math.Matrix.Identity;
for (int i = 0; i < sourceWorld.NumCollisionObjects; i++)
{
CollisionObject co = sourceWorld.CollisionObjectArray[i];
if (co != null && co.UserObject is BRigidBody)
{
BRigidBody rb = (BRigidBody)co.UserObject;
float mass = rb.isDynamic() ? rb.mass : 0f;
BCollisionShape existingShape = rb.GetComponent <BCollisionShape>();
CollisionShape shape = null;
if (existingShape is BSphereShape)
{
shape = ((BSphereShape)existingShape).CopyCollisionShape();
}
else if (existingShape is BBoxShape)
{
shape = ((BBoxShape)existingShape).CopyCollisionShape();
}
RigidBody bulletRB = null;
BulletSharp.Math.Vector3 localInertia = new BulletSharp.Math.Vector3();
rb.CreateOrConfigureRigidBody(ref bulletRB, ref localInertia, shape, null);
BulletSharp.Math.Vector3 pos = rb.GetCollisionObject().WorldTransform.Origin;
BulletSharp.Math.Quaternion rot = rb.GetCollisionObject().WorldTransform.GetOrientation();
BulletSharp.Math.Matrix.AffineTransformation(1f, ref rot, ref pos, out mm);
bulletRB.WorldTransform = mm;
World.AddRigidBody(bulletRB, rb.groupsIBelongTo, rb.collisionMask);
if (rb == ballRb)
{
bulletBallRb = bulletRB;
bulletRB.ApplyCentralImpulse(ballThrowForce.ToBullet());
}
}
}
//Step the simulation numberOfSimulationSteps times
for (int i = 0; i < numberOfSimulationSteps; i++)
{
int numSteps = World.StepSimulation(1f / 60f, 10, 1f / 60f);
ballPositions.Add(bulletBallRb.WorldTransform.Origin.ToUnity());
}
UnityEngine.Debug.Log("ExitPhysics");
if (World != null)
{
//remove/dispose constraints
int i;
for (i = World.NumConstraints - 1; i >= 0; i--)
{
TypedConstraint constraint = World.GetConstraint(i);
World.RemoveConstraint(constraint);
constraint.Dispose();
}
//remove the rigidbodies from the dynamics world and delete them
for (i = World.NumCollisionObjects - 1; i >= 0; i--)
{
CollisionObject obj = World.CollisionObjectArray[i];
RigidBody body = obj as RigidBody;
if (body != null && body.MotionState != null)
{
body.MotionState.Dispose();
}
World.RemoveCollisionObject(obj);
obj.Dispose();
}
World.Dispose();
Broadphase.Dispose();
Dispatcher.Dispose();
CollisionConf.Dispose();
}
if (Broadphase != null)
{
Broadphase.Dispose();
}
if (Dispatcher != null)
{
Dispatcher.Dispose();
}
if (CollisionConf != null)
{
CollisionConf.Dispose();
}
return(ballPositions);
}
public CollisionWorld(int staticBodyCount, int dynamicBodyCount)
{
m_PhysicsBodies = new NativeArray <PhysicsBody>(staticBodyCount + dynamicBodyCount, Allocator.Persistent, NativeArrayOptions.UninitializedMemory);
Broadphase = new Broadphase(staticBodyCount, dynamicBodyCount);
}
public void RemoveForce(ForceField field)
{
Broadphase.RemoveBody(field.Area);
}
// Has broadphase find all contacts and call AddContact on the
// ContactManager for each pair found
public void FindNewContacts()
{
Broadphase.UpdatePairs();
}
// Remove contacts without broadphase overlap
// Solves contact manifolds
public void TestCollisions()
{
for (int h = 0; h < ContactList.Count; h++)
{
var constraint = ContactList[h];
Box A = constraint.A;
Box B = constraint.B;
Body bodyA = A.body;
Body bodyB = B.body;
constraint.Flags &= ~ContactFlags.eIsland;
if (!bodyA.IsAwake() && !bodyB.IsAwake())
{
continue;
}
if (!bodyA.CanCollide(bodyB))
{
RemoveContact(constraint);
continue;
}
// Check if contact should persist
if (!Broadphase.TestOverlap(A.broadPhaseIndex, B.broadPhaseIndex))
{
RemoveContact(constraint);
continue;
}
Manifold manifold = constraint.manifold;
Manifold oldManifold = constraint.manifold;
Vec3 ot0 = oldManifold.tangentVectors;
Vec3 ot1 = oldManifold.bitangentVectors;
constraint.SolveCollision();
AABB.ComputeBasis(manifold.normal, ref manifold.tangentVectors, ref manifold.bitangentVectors);
for (int i = 0; i < manifold.contactCount; ++i)
{
Contact c = manifold.contacts[i];
c.tangentImpulse = c.bitangentImpulse = c.normalImpulse = 0;
byte oldWarmStart = c.warmStarted;
c.warmStarted = 0;
for (int j = 0; j < oldManifold.contactCount; ++j)
{
Contact oc = oldManifold.contacts[j];
if (c.fp.key == oc.fp.key)
{
c.normalImpulse = oc.normalImpulse;
// Attempt to re-project old friction solutions
Vec3 friction = ot0 * oc.tangentImpulse + ot1 * oc.bitangentImpulse;
c.tangentImpulse = Vec3.Dot(friction, manifold.tangentVectors);
c.bitangentImpulse = Vec3.Dot(friction, manifold.bitangentVectors);
c.warmStarted = Math.Max(oldWarmStart, (byte)(oldWarmStart + 1));
break;
}
}
}
if (ContactListener != null)
{
var now_colliding = constraint.Flags & ContactFlags.eColliding;
var was_colliding = constraint.Flags & ContactFlags.eWasColliding;
if (now_colliding > 0 && was_colliding == 0)
{
ContactListener.BeginContact(constraint);
}
else if (now_colliding == 0 && was_colliding > 0)
{
ContactListener.EndContact(constraint);
}
}
}
}
public List <BroadphaseProxy> QueryAABB(ref AABB aabb)
{
return(Broadphase.QueryAABB(ref aabb));
}
