moisesjpelaez - Physics Improvements

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

@@ -2,11 +2,13 @@ package leenkx.trait.physics.bullet;
 
 #if lnx_bullet
 
+import leenkx.math.Helper;
+import iron.data.MeshData;
 import iron.math.Vec4;
 import iron.math.Quat;
 import iron.object.Transform;
 import iron.object.MeshObject;
-import iron.data.MeshData;
+import iron.system.Time;
 
 /**
    RigidBody is used to allow objects to interact with Physics in your game including collisions and gravity.
@@ -76,6 +78,14 @@ class RigidBody extends iron.Trait {
 	static var triangleMeshCache = new Map<MeshData, bullet.Bt.TriangleMesh>();
 	static var usersCache = new Map<MeshData, Int>();
 
+	// Interpolation
+	var interpolate: Bool = false;
+	var time: Float = 0.0;
+	var currentPos: bullet.Bt.Vector3 = new bullet.Bt.Vector3(0, 0, 0);
+	var prevPos: bullet.Bt.Vector3 = new bullet.Bt.Vector3(0, 0, 0);
+	var currentRot: bullet.Bt.Quaternion = new bullet.Bt.Quaternion(0, 0, 0, 1);
+	var prevRot: bullet.Bt.Quaternion = new bullet.Bt.Quaternion(0, 0, 0, 1);
+
 	public function new(shape = Shape.Box, mass = 1.0, friction = 0.5, restitution = 0.0, group = 1, mask = 1,
 						params: RigidBodyParams = null, flags: RigidBodyFlags = null) {
 		super();
@@ -85,7 +95,7 @@ class RigidBody extends iron.Trait {
 			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);
+			quat1 = new bullet.Bt.Quaternion(0, 0, 0, 1);
 			trans1 = new bullet.Bt.Transform();
 			trans2 = new bullet.Bt.Transform();
 		}
@@ -117,6 +127,7 @@ class RigidBody extends iron.Trait {
 			animated: false,
 			trigger: false,
 			ccd: false,
+			interpolate: false,
 			staticObj: false,
 			useDeactivation: true
 		};
@@ -131,6 +142,7 @@ class RigidBody extends iron.Trait {
 		this.animated = flags.animated;
 		this.trigger = flags.trigger;
 		this.ccd = flags.ccd;
+		this.interpolate = flags.interpolate;
 		this.staticObj = flags.staticObj;
 		this.useDeactivation = flags.useDeactivation;
 
@@ -245,6 +257,9 @@ class RigidBody extends iron.Trait {
 		quat1.setValue(quat.x, quat.y, quat.z, quat.w);
 		trans1.setRotation(quat1);
 
+		currentPos.setValue(vec1.x(), vec1.y(), vec1.z());
+		currentRot.setValue(quat.x, quat.y, quat.z, quat.w);
+
 		var centerOfMassOffset = trans2;
 		centerOfMassOffset.setIdentity();
 		motionState = new bullet.Bt.DefaultMotionState(trans1, centerOfMassOffset);
@@ -308,7 +323,8 @@ class RigidBody extends iron.Trait {
 
 		physics.addRigidBody(this);
 		notifyOnRemove(removeFromWorld);
-
+		if (!animated) notifyOnUpdate(update);
+		
 		if (onReady != null) onReady();
 
 		#if js
@@ -318,26 +334,71 @@ class RigidBody extends iron.Trait {
 		#end
 	}
 
+
+	function update() {
+		if (interpolate) {
+			time += Time.delta;
+
+			while (time >= Time.fixedStep) {
+				time -= Time.fixedStep;
+			}
+
+			var t: Float = time / Time.fixedStep;
+			t = Helper.clamp(t, 0, 1);
+
+			var tx: Float = prevPos.x() * (1.0 - t) + currentPos.x() * t;
+			var ty: Float = prevPos.y() * (1.0 - t) + currentPos.y() * t;
+			var tz: Float = prevPos.z() * (1.0 - t) + currentPos.z() * t;
+
+			var tRot: bullet.Bt.Quaternion = nlerp(prevRot, currentRot, t);
+
+			transform.loc.set(tx, ty, tz, 1.0);
+			transform.rot.set(tRot.x(), tRot.y(), tRot.z(), tRot.w());
+		} else {
+			transform.loc.set(currentPos.x(), currentPos.y(), currentPos.z(), 1.0);
+			transform.rot.set(currentRot.x(), currentRot.y(), currentRot.z(), currentRot.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();
+	}
+
+	function nlerp(q1: bullet.Bt.Quaternion, q2: bullet.Bt.Quaternion, t: Float): bullet.Bt.Quaternion {
+		var dot = q1.x() * q2.x() + q1.y() * q2.y() + q1.z() * q2.z() + q1.w() * q2.w();
+		var _q2 = dot < 0 ? new bullet.Bt.Quaternion(-q2.x(), -q2.y(), -q2.z(), -q2.w()) : q2;
+
+		var x = q1.x() * (1.0 - t) + _q2.x() * t;
+		var y = q1.y() * (1.0 - t) + _q2.y() * t;
+		var z = q1.z() * (1.0 - t) + _q2.z() * t;
+		var w = q1.w() * (1.0 - t) + _q2.w() * t;
+
+		var len = Math.sqrt(x * x + y * y + z * z + w * w);
+		return new bullet.Bt.Quaternion(x / len, y / len, z / len, w / len);
+	}
+	
 	function physicsUpdate() {
 		if (!ready) return;
 		if (animated) {
 			syncTransform();
-		}
+		} else {
-		else {
+			if (interpolate) {
+				prevPos.setValue(currentPos.x(), currentPos.y(), currentPos.z());
+				prevRot.setValue(currentRot.x(), currentRot.y(), currentRot.z(), currentRot.w());
+			}
 			var trans = body.getWorldTransform();
 			var p = trans.getOrigin();
 			var q = trans.getRotation();
-
-			transform.loc.set(p.x(), p.y(), p.z());
-			transform.rot.set(q.x(), q.y(), q.z(), q.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.clearDelta();
-			transform.buildMatrix();
+			// transform.buildMatrix();
+			currentPos.setValue(p.x(), p.y(), p.z());
+			currentRot.setValue(q.x(), q.y(), q.z(), q.w());
+
 
 			#if hl
 			p.delete();
@@ -690,6 +751,7 @@ typedef RigidBodyFlags = {
 	var animated: Bool;
 	var trigger: Bool;
 	var ccd: Bool;
+	var interpolate: Bool;
 	var staticObj: Bool;
 	var useDeactivation: Bool;
 }