360 lines
9.8 KiB
Haxe
360 lines
9.8 KiB
Haxe
package armory.trait.physics.bullet;
|
|
|
|
#if arm_bullet
|
|
|
|
import iron.Trait;
|
|
import iron.system.Time;
|
|
import iron.math.Vec4;
|
|
import iron.math.RayCaster;
|
|
import iron.data.SceneFormat;
|
|
|
|
class ContactPair {
|
|
public var a:Int;
|
|
public var b:Int;
|
|
public var posA:Vec4;
|
|
public var posB:Vec4;
|
|
public var normOnB:Vec4;
|
|
public var impulse:Float;
|
|
public var distance:Float;
|
|
public function new(a:Int, b:Int) {
|
|
this.a = a;
|
|
this.b = b;
|
|
}
|
|
}
|
|
|
|
class PhysicsWorld extends Trait {
|
|
|
|
public static var active:PhysicsWorld = null;
|
|
static var sceneRemoved = false;
|
|
|
|
#if arm_physics_soft
|
|
public var world:bullet.Bt.SoftRigidDynamicsWorld;
|
|
#else
|
|
public var world:bullet.Bt.DiscreteDynamicsWorld;
|
|
#end
|
|
|
|
var dispatcher:bullet.Bt.CollisionDispatcher;
|
|
var gimpactRegistered = false;
|
|
var contacts:Array<ContactPair>;
|
|
var preUpdates:Array<Void->Void> = null;
|
|
public var rbMap:Map<Int, RigidBody>;
|
|
public var timeScale = 1.0;
|
|
var timeStep = 1 / 60;
|
|
var maxSteps = 1;
|
|
public var solverIterations = 10;
|
|
public var hitPointWorld = new Vec4();
|
|
public var hitNormalWorld = new Vec4();
|
|
var pairCache:Bool = false;
|
|
|
|
static var nullvec = true;
|
|
var vec1:bullet.Bt.Vector3 = null;
|
|
var vec2:bullet.Bt.Vector3 = null;
|
|
|
|
#if arm_debug
|
|
public static var physTime = 0.0;
|
|
#end
|
|
|
|
public function new(timeScale = 1.0, timeStep = 1 / 60, solverIterations = 10) {
|
|
super();
|
|
|
|
if (nullvec) {
|
|
nullvec = false;
|
|
vec1 = new bullet.Bt.Vector3(0, 0, 0);
|
|
vec2 = new bullet.Bt.Vector3(0, 0, 0);
|
|
}
|
|
|
|
// Scene spawn
|
|
if (active != null && !sceneRemoved) return;
|
|
sceneRemoved = false;
|
|
|
|
this.timeScale = timeScale;
|
|
this.timeStep = timeStep;
|
|
maxSteps = timeStep < 1 / 60 ? 10 : 1;
|
|
this.solverIterations = solverIterations;
|
|
|
|
// First scene
|
|
if (active == null) {
|
|
createPhysics();
|
|
}
|
|
// Scene switch
|
|
else {
|
|
world = active.world;
|
|
dispatcher = active.dispatcher;
|
|
gimpactRegistered = active.gimpactRegistered;
|
|
}
|
|
|
|
contacts = [];
|
|
rbMap = new Map();
|
|
active = this;
|
|
|
|
// Ensure physics are updated first in the lateUpdate list
|
|
_lateUpdate = [lateUpdate];
|
|
@:privateAccess iron.App.traitLateUpdates.insert(0, lateUpdate);
|
|
|
|
iron.Scene.active.notifyOnRemove(function() {
|
|
sceneRemoved = true;
|
|
});
|
|
}
|
|
|
|
public function reset() {
|
|
for (rb in active.rbMap) removeRigidBody(rb);
|
|
}
|
|
|
|
function createPhysics() {
|
|
var broadphase = new bullet.Bt.DbvtBroadphase();
|
|
|
|
#if arm_physics_soft
|
|
var collisionConfiguration = new bullet.Bt.SoftBodyRigidBodyCollisionConfiguration();
|
|
#else
|
|
var collisionConfiguration = new bullet.Bt.DefaultCollisionConfiguration();
|
|
#end
|
|
|
|
dispatcher = new bullet.Bt.CollisionDispatcher(collisionConfiguration);
|
|
var solver = new bullet.Bt.SequentialImpulseConstraintSolver();
|
|
|
|
var g = iron.Scene.active.raw.gravity;
|
|
var gravity = g == null ? new Vec4(0, 0, -9.81) : new Vec4(g[0], g[1], g[2]);
|
|
|
|
#if arm_physics_soft
|
|
var softSolver = new bullet.Bt.DefaultSoftBodySolver();
|
|
world = new bullet.Bt.SoftRigidDynamicsWorld(dispatcher, broadphase, solver, collisionConfiguration, softSolver);
|
|
vec1.setX(gravity.x);
|
|
vec1.setY(gravity.y);
|
|
vec1.setZ(gravity.z);
|
|
#if js
|
|
world.getWorldInfo().set_m_gravity(vec1);
|
|
#else
|
|
world.getWorldInfo().m_gravity = vec1;
|
|
#end
|
|
#else
|
|
world = new bullet.Bt.DiscreteDynamicsWorld(dispatcher, broadphase, solver, collisionConfiguration);
|
|
#end
|
|
|
|
setGravity(gravity);
|
|
}
|
|
|
|
public function setGravity(v:Vec4) {
|
|
vec1.setValue(v.x, v.y, v.z);
|
|
world.setGravity(vec1);
|
|
}
|
|
|
|
public function addRigidBody(body:RigidBody) {
|
|
#if js
|
|
world.addRigidBodyToGroup(body.body, body.group, body.group);
|
|
#else
|
|
world.addRigidBody(body.body, body.group, body.group);
|
|
#end
|
|
rbMap.set(body.id, body);
|
|
}
|
|
|
|
public function removeRigidBody(body:RigidBody) {
|
|
if (body.destroyed) return;
|
|
body.destroyed = true;
|
|
if (world != null) world.removeRigidBody(body.body);
|
|
rbMap.remove(body.id);
|
|
#if js
|
|
bullet.Bt.Ammo.destroy(body.motionState);
|
|
bullet.Bt.Ammo.destroy(body.btshape);
|
|
bullet.Bt.Ammo.destroy(body.body);
|
|
#else
|
|
body.motionState.delete();
|
|
body.btshape.delete();
|
|
body.body.delete();
|
|
#end
|
|
}
|
|
|
|
// public function addKinematicCharacterController(controller:KinematicCharacterController) {
|
|
// if (!pairCache){ // Only create PairCache if needed
|
|
// world.getPairCache().setInternalGhostPairCallback(BtGhostPairCallbackPointer.create());
|
|
// pairCache = true;
|
|
// }
|
|
// world.addAction(controller.character);
|
|
// world.addCollisionObjectToGroup(controller.body, controller.group, controller.group);
|
|
// }
|
|
|
|
// public function removeKinematicCharacterController(controller:KinematicCharacterController) {
|
|
// if (world != null) {
|
|
// world.removeCollisionObject(controller.body);
|
|
// world.removeAction(controller.character);
|
|
// }
|
|
// #if js
|
|
// bullet.Bt.Ammo.destroy(controller.body);
|
|
// #else
|
|
// var cbody = controller.body;
|
|
// untyped __cpp__("delete cbody");
|
|
// #end
|
|
// }
|
|
|
|
/**
|
|
* Used to get intersecting rigid bodies with the passed in RigidBody as reference. Often used when checking for object collisions.
|
|
*
|
|
* @param body The passed in RigidBody to be checked for intersecting rigid bodies.
|
|
* @return Array<RigidBody> or null.
|
|
*/
|
|
public function getContacts(body:RigidBody):Array<RigidBody> {
|
|
if (contacts.length == 0) return null;
|
|
var res:Array<RigidBody> = [];
|
|
for (i in 0...contacts.length) {
|
|
var c = contacts[i];
|
|
#if js
|
|
|
|
if (c.a == untyped body.body.userIndex) res.push(rbMap.get(c.b));
|
|
else if (c.b == untyped body.body.userIndex) res.push(rbMap.get(c.a));
|
|
|
|
#else
|
|
|
|
var ob:bullet.Bt.CollisionObject = body.body;
|
|
if (c.a == ob.getUserIndex()) res.push(rbMap.get(c.b));
|
|
else if (c.b == ob.getUserIndex()) res.push(rbMap.get(c.a));
|
|
|
|
#end
|
|
}
|
|
return res;
|
|
}
|
|
|
|
public function getContactPairs(body:RigidBody):Array<ContactPair> {
|
|
if (contacts.length == 0) return null;
|
|
var res:Array<ContactPair> = [];
|
|
for (i in 0...contacts.length) {
|
|
var c = contacts[i];
|
|
#if js
|
|
|
|
if (c.a == untyped body.body.userIndex) res.push(c);
|
|
else if (c.b == untyped body.body.userIndex) res.push(c);
|
|
|
|
#else
|
|
|
|
var ob:bullet.Bt.CollisionObject = body.body;
|
|
if (c.a == ob.getUserIndex()) res.push(c);
|
|
else if (c.b == ob.getUserIndex()) res.push(c);
|
|
|
|
#end
|
|
}
|
|
return res;
|
|
}
|
|
|
|
function lateUpdate() {
|
|
var t = Time.delta * timeScale;
|
|
if (t == 0.0) return; // Simulation paused
|
|
|
|
#if arm_debug
|
|
var startTime = kha.Scheduler.realTime();
|
|
#end
|
|
|
|
if (preUpdates != null) for (f in preUpdates) f();
|
|
|
|
world.stepSimulation(timeStep, maxSteps, t);
|
|
updateContacts();
|
|
|
|
for (rb in rbMap) @:privateAccess rb.physicsUpdate();
|
|
|
|
#if arm_debug
|
|
physTime = kha.Scheduler.realTime() - startTime;
|
|
#end
|
|
}
|
|
|
|
function updateContacts() {
|
|
contacts = [];
|
|
|
|
var disp:bullet.Bt.Dispatcher = dispatcher;
|
|
var numManifolds = disp.getNumManifolds();
|
|
|
|
for (i in 0...numManifolds) {
|
|
var contactManifold = disp.getManifoldByIndexInternal(i);
|
|
#if js
|
|
var body0 = untyped bullet.Bt.Ammo.btRigidBody.prototype.upcast(contactManifold.getBody0());
|
|
var body1 = untyped bullet.Bt.Ammo.btRigidBody.prototype.upcast(contactManifold.getBody1());
|
|
#else
|
|
var body0:bullet.Bt.CollisionObject = contactManifold.getBody0();
|
|
var body1:bullet.Bt.CollisionObject = contactManifold.getBody1();
|
|
#end
|
|
|
|
var numContacts = contactManifold.getNumContacts();
|
|
var pt:bullet.Bt.ManifoldPoint = null;
|
|
var posA:bullet.Bt.Vector3 = null;
|
|
var posB:bullet.Bt.Vector3 = null;
|
|
var nor:bullet.Bt.Vector3 = null;
|
|
var cp:ContactPair = null;
|
|
for (j in 0...numContacts) {
|
|
pt = contactManifold.getContactPoint(j);
|
|
#if js
|
|
posA = pt.get_m_positionWorldOnA();
|
|
posB = pt.get_m_positionWorldOnB();
|
|
nor = pt.get_m_normalWorldOnB();
|
|
cp = new ContactPair(untyped body0.userIndex, untyped body1.userIndex);
|
|
#else
|
|
posA = pt.m_positionWorldOnA;
|
|
posB = pt.m_positionWorldOnB;
|
|
nor = pt.m_normalWorldOnB;
|
|
cp = new ContactPair(body0.getUserIndex(), body1.getUserIndex());
|
|
#end
|
|
cp.posA = new Vec4(posA.x(), posA.y(), posA.z());
|
|
cp.posB = new Vec4(posB.x(), posB.y(), posB.z());
|
|
cp.normOnB = new Vec4(nor.x(), nor.y(), nor.z());
|
|
cp.impulse = pt.getAppliedImpulse();
|
|
cp.distance = pt.getDistance();
|
|
contacts.push(cp);
|
|
}
|
|
}
|
|
}
|
|
|
|
public function pickClosest(inputX:Float, inputY:Float):RigidBody {
|
|
var camera = iron.Scene.active.camera;
|
|
var start = new Vec4();
|
|
var end = new Vec4();
|
|
RayCaster.getDirection(start, end, inputX, inputY, camera);
|
|
return rayCast(camera.transform.world.getLoc(), end);
|
|
}
|
|
|
|
public function rayCast(from:Vec4, to:Vec4):RigidBody {
|
|
var rayFrom = vec1;
|
|
var rayTo = vec2;
|
|
rayFrom.setValue(from.x, from.y, from.z);
|
|
rayTo.setValue(to.x, to.y, to.z);
|
|
|
|
var rayCallback = new bullet.Bt.ClosestRayResultCallback(rayFrom, rayTo);
|
|
var worldDyn:bullet.Bt.DynamicsWorld = world;
|
|
var worldCol:bullet.Bt.CollisionWorld = worldDyn;
|
|
worldCol.rayTest(rayFrom, rayTo, rayCallback);
|
|
var rb:RigidBody = null;
|
|
|
|
var rc:bullet.Bt.RayResultCallback = rayCallback;
|
|
if (rc.hasHit()) {
|
|
#if js
|
|
var co = rayCallback.get_m_collisionObject();
|
|
var body = untyped bullet.Bt.Ammo.btRigidBody.prototype.upcast(co);
|
|
var hit = rayCallback.get_m_hitPointWorld();
|
|
hitPointWorld.set(hit.x(), hit.y(), hit.z());
|
|
var norm = rayCallback.get_m_hitNormalWorld();
|
|
hitNormalWorld.set(norm.x(), norm.y(), norm.z());
|
|
rb = rbMap.get(untyped body.userIndex);
|
|
#elseif cpp
|
|
var hit = rayCallback.m_hitPointWorld;
|
|
hitPointWorld.set(hit.x(), hit.y(), hit.z());
|
|
var norm = rayCallback.m_hitNormalWorld;
|
|
hitNormalWorld.set(norm.x(), norm.y(), norm.z());
|
|
rb = rbMap.get(rayCallback.m_collisionObject.getUserIndex());
|
|
#end
|
|
}
|
|
|
|
#if js
|
|
bullet.Bt.Ammo.destroy(rayCallback);
|
|
#else
|
|
rayCallback.delete();
|
|
#end
|
|
|
|
return rb;
|
|
}
|
|
|
|
public function notifyOnPreUpdate(f:Void->Void) {
|
|
if (preUpdates == null) preUpdates = [];
|
|
preUpdates.push(f);
|
|
}
|
|
|
|
public function removePreUpdate(f:Void->Void) {
|
|
preUpdates.remove(f);
|
|
}
|
|
}
|
|
|
|
#end
|