armory/Sources/armory/trait/physics/bullet/RigidBody.hx

606 lines
17 KiB
Haxe

package armory.trait.physics.bullet;
#if arm_bullet
import iron.math.Vec4;
import iron.math.Quat;
import iron.object.Transform;
import iron.object.MeshObject;
import iron.data.MeshData;
/**
* RigidBody is used to allow objects to interact with Physics in your game including collisions and gravity.
* RigidBody can also be used with the getContacts method to detect collisions and run appropriate code.
* The Bullet physics engine is used for these calculations.
*/
@:access(armory.trait.physics.bullet.PhysicsWorld)
class RigidBody extends iron.Trait {
var shape:Shape;
public var physics:PhysicsWorld;
public var transform:Transform = null;
public var mass:Float;
public var friction:Float;
public var restitution:Float;
public var collisionMargin:Float;
public var linearDamping:Float;
public var angularDamping:Float;
public var animated:Bool;
public var staticObj:Bool;
public var destroyed = false;
var linearFactors:Array<Float>;
var angularFactors:Array<Float>;
var deactivationParams:Array<Float>;
var ccd = false; // Continuous collision detection
public var group = 1;
public var mask = 1;
var trigger = false;
var bodyScaleX:Float; // Transform scale at creation time
var bodyScaleY:Float;
var bodyScaleZ:Float;
var currentScaleX:Float;
var currentScaleY:Float;
var currentScaleZ:Float;
public var body:bullet.Bt.RigidBody = null;
public var motionState:bullet.Bt.MotionState;
public var btshape:bullet.Bt.CollisionShape;
public var ready = false;
static var nextId = 0;
public var id = 0;
public var onReady:Void->Void = null;
public var onContact:Array<RigidBody->Void> = null;
public var heightData:haxe.io.Bytes = null;
#if js
static var ammoArray:Int = -1;
#end
static var nullvec = true;
static var vec1:bullet.Bt.Vector3;
static var vec2:bullet.Bt.Vector3;
static var vec3:bullet.Bt.Vector3;
static var quat1:bullet.Bt.Quaternion;
static var trans1:bullet.Bt.Transform;
static var trans2:bullet.Bt.Transform;
static var quat = new Quat();
static var CF_STATIC_OBJECT= 1;
static var CF_KINEMATIC_OBJECT= 2;
static var CF_NO_CONTACT_RESPONSE = 4;
static var CF_CHARACTER_OBJECT = 16;
static var convexHullCache = new Map<MeshData, bullet.Bt.ConvexHullShape>();
static var triangleMeshCache = new Map<MeshData, bullet.Bt.TriangleMesh>();
static var usersCache = new Map<MeshData, Int>();
public function new(shape = Shape.Box, mass = 1.0, friction = 0.5, restitution = 0.0, group = 1, mask=1,
params:Array<Float> = null, flags:Array<Bool> = null) {
super();
if (nullvec) {
nullvec = false;
vec1 = new bullet.Bt.Vector3(0, 0, 0);
vec2 = new bullet.Bt.Vector3(0, 0, 0);
vec3 = new bullet.Bt.Vector3(0, 0, 0);
quat1 = new bullet.Bt.Quaternion(0, 0, 0, 0);
trans1 = new bullet.Bt.Transform();
trans2 = new bullet.Bt.Transform();
}
this.shape = shape;
this.mass = mass;
this.friction = friction;
this.restitution = restitution;
this.group = group;
this.mask = mask;
if (params == null) params = [0.04, 0.1, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0];
if (flags == null) flags = [false, false, false];
this.linearDamping = params[0];
this.angularDamping = params[1];
this.linearFactors = [params[2], params[3], params[4]];
this.angularFactors = [params[5], params[6], params[7]];
this.collisionMargin = params[8];
this.deactivationParams = [params[9], params[10], params[11]];
this.animated = flags[0];
this.trigger = flags[1];
this.ccd = flags[2];
this.staticObj = flags[3];
notifyOnAdd(init);
}
inline function withMargin(f:Float) {
return f - f * collisionMargin;
}
public function notifyOnReady(f:Void->Void) {
onReady = f;
if (ready) onReady();
}
public function init() {
if (ready) return;
ready = true;
if (!Std.is(object, MeshObject)) return; // No mesh data
transform = object.transform;
physics = armory.trait.physics.PhysicsWorld.active;
if (shape == Shape.Box) {
vec1.setX(withMargin(transform.dim.x / 2));
vec1.setY(withMargin(transform.dim.y / 2));
vec1.setZ(withMargin(transform.dim.z / 2));
btshape = new bullet.Bt.BoxShape(vec1);
}
else if (shape == Shape.Sphere) {
btshape = new bullet.Bt.SphereShape(withMargin(transform.dim.x / 2));
}
else if (shape == Shape.ConvexHull) {
var shapeConvex = fillConvexHull(transform.scale, collisionMargin);
btshape = shapeConvex;
}
else if (shape == Shape.Cone) {
var coneZ = new bullet.Bt.ConeShapeZ(
withMargin(transform.dim.x / 2), // Radius
withMargin(transform.dim.z)); // Height
var cone:bullet.Bt.ConeShape = coneZ;
btshape = cone;
}
else if (shape == Shape.Cylinder) {
vec1.setX(withMargin(transform.dim.x / 2));
vec1.setY(withMargin(transform.dim.y / 2));
vec1.setZ(withMargin(transform.dim.z / 2));
var cylZ = new bullet.Bt.CylinderShapeZ(vec1);
var cyl:bullet.Bt.CylinderShape = cylZ;
btshape = cyl;
}
else if (shape == Shape.Capsule) {
var r = transform.dim.x / 2;
var capsZ = new bullet.Bt.CapsuleShapeZ(
withMargin(r), // Radius
withMargin(transform.dim.z - r * 2)); // Height between 2 sphere centers
var caps:bullet.Bt.CapsuleShape = capsZ;
btshape = caps;
}
else if (shape == Shape.Mesh) {
var meshInterface = fillTriangleMesh(transform.scale);
if (mass > 0) {
var shapeGImpact = new bullet.Bt.GImpactMeshShape(meshInterface);
shapeGImpact.updateBound();
var shapeConcave:bullet.Bt.ConcaveShape = shapeGImpact;
btshape = shapeConcave;
if (!physics.gimpactRegistered) {
#if js
new bullet.Bt.GImpactCollisionAlgorithm().registerAlgorithm(physics.dispatcher);
#else
shapeGImpact.registerAlgorithm(physics.dispatcher);
#end
physics.gimpactRegistered = true;
}
}
else {
var shapeBvh = new bullet.Bt.BvhTriangleMeshShape(meshInterface, true, true);
var shapeTri:bullet.Bt.TriangleMeshShape = shapeBvh;
var shapeConcave:bullet.Bt.ConcaveShape = shapeTri;
btshape = shapeConcave;
}
}
else if (shape == Shape.Terrain) {
#if js
var length = heightData.length;
if (ammoArray == -1) {
ammoArray = bullet.Bt.Ammo._malloc(length);
}
// From texture bytes
for (i in 0...length) {
bullet.Bt.Ammo.HEAPU8[ammoArray + i] = heightData.get(i);
}
var slice = Std.int(Math.sqrt(length)); // Assuming square terrain data
var axis = 2; // z
var dataType = 5; // u8
btshape = new bullet.Bt.HeightfieldTerrainShape(slice, slice, ammoArray, 1 / 255, 0, 1, axis, dataType, false);
vec1.setX(transform.dim.x / slice);
vec1.setY(transform.dim.y / slice);
vec1.setZ(transform.dim.z);
btshape.setLocalScaling(vec1);
#end
}
trans1.setIdentity();
vec1.setX(transform.worldx());
vec1.setY(transform.worldy());
vec1.setZ(transform.worldz());
trans1.setOrigin(vec1);
quat.fromMat(transform.world);
quat1.setValue(quat.x, quat.y, quat.z, quat.w);
trans1.setRotation(quat1);
var centerOfMassOffset = trans2;
centerOfMassOffset.setIdentity();
motionState = new bullet.Bt.DefaultMotionState(trans1, centerOfMassOffset);
vec1.setX(0);
vec1.setY(0);
vec1.setZ(0);
var inertia = vec1;
if(staticObj || animated) mass = 0;
if (mass > 0) btshape.calculateLocalInertia(mass, inertia);
var bodyCI = new bullet.Bt.RigidBodyConstructionInfo(mass, motionState, btshape, inertia);
body = new bullet.Bt.RigidBody(bodyCI);
var bodyColl:bullet.Bt.CollisionObject = body;
bodyColl.setFriction(friction);
// body.setRollingFriction(friction); // This causes bodies to get stuck, apply angular damping instead
if (shape == Shape.Sphere || shape == Shape.Cylinder || shape == Shape.Cone || shape == Shape.Capsule) {
angularDamping += friction;
}
bodyColl.setRestitution(restitution);
if (deactivationParams != null) {
setDeactivationParams(deactivationParams[0], deactivationParams[1], deactivationParams[2]);
}
else {
setActivationState(ActivationState.NoDeactivation);
}
if (linearDamping != 0.04 || angularDamping != 0.1) {
body.setDamping(linearDamping, angularDamping);
}
if (linearFactors != null) {
setLinearFactor(linearFactors[0], linearFactors[1], linearFactors[2]);
}
if (angularFactors != null) {
setAngularFactor(angularFactors[0], angularFactors[1], angularFactors[2]);
}
if (trigger) bodyColl.setCollisionFlags(bodyColl.getCollisionFlags() | CF_NO_CONTACT_RESPONSE);
if (animated){
bodyColl.setCollisionFlags(bodyColl.getCollisionFlags() | CF_KINEMATIC_OBJECT);
bodyColl.setCollisionFlags(bodyColl.getCollisionFlags() & ~CF_STATIC_OBJECT);
}
if (staticObj && !animated) bodyColl.setCollisionFlags(bodyColl.getCollisionFlags() | CF_STATIC_OBJECT);
if (ccd) setCcd(transform.radius);
bodyScaleX = currentScaleX = transform.scale.x;
bodyScaleY = currentScaleY = transform.scale.y;
bodyScaleZ = currentScaleZ = transform.scale.z;
id = nextId;
nextId++;
#if js
//body.setUserIndex(nextId);
untyped body.userIndex = id;
#else
bodyColl.setUserIndex(id);
#end
physics.addRigidBody(this);
notifyOnRemove(removeFromWorld);
if (onReady != null) onReady();
#if js
bullet.Bt.Ammo.destroy(bodyCI);
#else
bodyCI.delete();
#end
}
function physicsUpdate() {
if (!ready) return;
if (animated) {
syncTransform();
}
else {
var bodyColl:bullet.Bt.CollisionObject = body;
var trans = bodyColl.getWorldTransform();
var p = trans.getOrigin();
var q = trans.getRotation();
var qw:bullet.Bt.QuadWord = q;
transform.loc.set(p.x(), p.y(), p.z());
transform.rot.set(qw.x(), qw.y(), qw.z(), qw.w());
if (object.parent != null) {
var ptransform = object.parent.transform;
transform.loc.x -= ptransform.worldx();
transform.loc.y -= ptransform.worldy();
transform.loc.z -= ptransform.worldz();
}
transform.buildMatrix();
}
if (onContact != null) {
var rbs = physics.getContacts(this);
if (rbs != null) for (rb in rbs) for (f in onContact) f(rb);
}
}
public function removeFromWorld() {
if (physics != null) physics.removeRigidBody(this);
}
public function activate() {
var bodyColl:bullet.Bt.CollisionObject = body;
bodyColl.activate(false);
}
public function disableGravity() {
vec1.setValue(0, 0, 0);
body.setGravity(vec1);
}
public function enableGravity() {
body.setGravity(physics.world.getGravity());
}
public function setGravity(v:Vec4) {
vec1.setValue(v.x, v.y, v.z);
body.setGravity(vec1);
}
public function setActivationState(newState:Int) {
var bodyColl:bullet.Bt.CollisionObject = body;
bodyColl.setActivationState(newState);
}
public function setDeactivationParams(linearThreshold:Float, angularThreshold:Float, time:Float) {
body.setSleepingThresholds(linearThreshold, angularThreshold);
// body.setDeactivationTime(time); // not available in ammo
}
public function applyForce(force:Vec4, loc:Vec4 = null) {
activate();
vec1.setValue(force.x, force.y, force.z);
if (loc == null) {
body.applyCentralForce(vec1);
}
else {
vec2.setValue(loc.x, loc.y, loc.z);
body.applyForce(vec1, vec2);
}
}
public function applyImpulse(impulse:Vec4, loc:Vec4 = null) {
activate();
vec1.setValue(impulse.x, impulse.y, impulse.z);
if (loc == null) {
body.applyCentralImpulse(vec1);
}
else {
vec2.setValue(loc.x, loc.y, loc.z);
body.applyImpulse(vec1, vec2);
}
}
public function applyTorque(torque:Vec4) {
activate();
vec1.setValue(torque.x, torque.y, torque.z);
body.applyTorque(vec1);
}
public function applyTorqueImpulse(torque:Vec4) {
activate();
vec1.setValue(torque.x, torque.y, torque.z);
body.applyTorqueImpulse(vec1);
}
public function setLinearFactor(x:Float, y:Float, z:Float) {
vec1.setValue(x, y, z);
body.setLinearFactor(vec1);
}
public function setAngularFactor(x:Float, y:Float, z:Float) {
vec1.setValue(x, y, z);
body.setAngularFactor(vec1);
}
public function getLinearVelocity():Vec4 {
var v = body.getLinearVelocity();
return new Vec4(v.x(), v.y(), v.z());
}
public function setLinearVelocity(x:Float, y:Float, z:Float) {
vec1.setValue(x, y, z);
body.setLinearVelocity(vec1);
}
public function getAngularVelocity():Vec4 {
var v = body.getAngularVelocity();
return new Vec4(v.x(), v.y(), v.z());
}
public function setAngularVelocity(x:Float, y:Float, z:Float) {
vec1.setValue(x, y, z);
body.setAngularVelocity(vec1);
}
public function setFriction(f:Float) {
var bodyColl:bullet.Bt.CollisionObject = body;
bodyColl.setFriction(f);
// bodyColl.setRollingFriction(f);
this.friction = f;
}
public function notifyOnContact(f:RigidBody->Void) {
if (onContact == null) onContact = [];
onContact.push(f);
}
public function removeContact(f:RigidBody->Void) {
onContact.remove(f);
}
function setScale(v:Vec4) {
currentScaleX = v.x;
currentScaleY = v.y;
currentScaleZ = v.z;
vec1.setX(v.x / bodyScaleX);
vec1.setY(v.y / bodyScaleY);
vec1.setZ(v.z / bodyScaleZ);
btshape.setLocalScaling(vec1);
var worldDyn:bullet.Bt.DynamicsWorld = physics.world;
var worldCol:bullet.Bt.CollisionWorld = worldDyn;
worldCol.updateSingleAabb(body);
}
public function syncTransform() {
var t = transform;
t.buildMatrix();
vec1.setValue(t.worldx(), t.worldy(), t.worldz());
trans1.setOrigin(vec1);
quat.fromMat(t.world);
quat1.setValue(quat.x, quat.y, quat.z, quat.w);
trans1.setRotation(quat1);
if(animated)
body.getMotionState().setWorldTransform(trans1);
else
body.setCenterOfMassTransform(trans1);
if (currentScaleX != t.scale.x || currentScaleY != t.scale.y || currentScaleZ != t.scale.z) setScale(t.scale);
activate();
}
// Continuous collision detection
public function setCcd(sphereRadius:Float, motionThreshold = 1e-7) {
var bodyColl:bullet.Bt.CollisionObject = body;
bodyColl.setCcdSweptSphereRadius(sphereRadius);
bodyColl.setCcdMotionThreshold(motionThreshold);
}
function fillConvexHull(scale:Vec4, margin:kha.FastFloat):bullet.Bt.ConvexHullShape {
// Check whether shape already exists
var data = cast(object, MeshObject).data;
var shape = convexHullCache.get(data);
if (shape != null) {
usersCache.set(data, usersCache.get(data) + 1);
return shape;
}
shape = new bullet.Bt.ConvexHullShape();
convexHullCache.set(data, shape);
usersCache.set(data, 1);
var positions = data.geom.positions;
var sx:kha.FastFloat = scale.x * (1.0 - margin) * (1 / 32767);
var sy:kha.FastFloat = scale.y * (1.0 - margin) * (1 / 32767);
var sz:kha.FastFloat = scale.z * (1.0 - margin) * (1 / 32767);
if (data.raw.scale_pos != null) {
sx *= data.raw.scale_pos;
sy *= data.raw.scale_pos;
sz *= data.raw.scale_pos;
}
for (i in 0...Std.int(positions.length / 4)) {
vec1.setX(positions[i * 4 ] * sx);
vec1.setY(positions[i * 4 + 1] * sy);
vec1.setZ(positions[i * 4 + 2] * sz);
shape.addPoint(vec1, true);
}
return shape;
}
function fillTriangleMesh(scale:Vec4):bullet.Bt.TriangleMesh {
// Check whether shape already exists
var data = cast(object, MeshObject).data;
var triangleMesh = triangleMeshCache.get(data);
if (triangleMesh != null) {
usersCache.set(data, usersCache.get(data) + 1);
return triangleMesh;
}
triangleMesh = new bullet.Bt.TriangleMesh(true, true);
triangleMeshCache.set(data, triangleMesh);
usersCache.set(data, 1);
var positions = data.geom.positions;
var indices = data.geom.indices;
var sx:kha.FastFloat = scale.x * (1 / 32767);
var sy:kha.FastFloat = scale.y * (1 / 32767);
var sz:kha.FastFloat = scale.z * (1 / 32767);
if (data.raw.scale_pos != null) {
sx *= data.raw.scale_pos;
sy *= data.raw.scale_pos;
sz *= data.raw.scale_pos;
}
for (ar in indices) {
for (i in 0...Std.int(ar.length / 3)) {
vec1.setX(positions[ar[i * 3 ] * 4 ] * sx);
vec1.setY(positions[ar[i * 3 ] * 4 + 1] * sy);
vec1.setZ(positions[ar[i * 3 ] * 4 + 2] * sz);
vec2.setX(positions[ar[i * 3 + 1] * 4 ] * sx);
vec2.setY(positions[ar[i * 3 + 1] * 4 + 1] * sy);
vec2.setZ(positions[ar[i * 3 + 1] * 4 + 2] * sz);
vec3.setX(positions[ar[i * 3 + 2] * 4 ] * sx);
vec3.setY(positions[ar[i * 3 + 2] * 4 + 1] * sy);
vec3.setZ(positions[ar[i * 3 + 2] * 4 + 2] * sz);
triangleMesh.addTriangle(vec1, vec2, vec3);
}
}
return triangleMesh;
}
public function delete() {
#if js
bullet.Bt.Ammo.destroy(motionState);
bullet.Bt.Ammo.destroy(body);
#else
motionState.delete();
body.delete();
#end
// Delete shape if no other user is found
if (shape == Shape.ConvexHull || shape == Shape.Mesh) {
var data = cast(object, MeshObject).data;
var i = usersCache.get(data) - 1;
usersCache.set(data, i);
if (i <= 0) {
deleteShape();
shape == Shape.ConvexHull ?
convexHullCache.remove(data) :
triangleMeshCache.remove(data);
}
}
else deleteShape();
}
inline function deleteShape() {
#if js
bullet.Bt.Ammo.destroy(btshape);
#else
btshape.delete();
#end
}
}
@:enum abstract Shape(Int) from Int to Int {
var Box = 0;
var Sphere = 1;
var ConvexHull = 2;
var Mesh = 3;
var Cone = 4;
var Cylinder = 5;
var Capsule = 6;
var Terrain = 7;
}
@:enum abstract ActivationState(Int) from Int to Int {
var Active = 1;
var NoDeactivation = 4;
var NoSimulation = 5;
}
#end