Update leenkx/Sources/leenkx/trait/physics/bullet/DebugDrawHelper.hx

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

@@ -1,5 +1,8 @@
 package leenkx.trait.physics.bullet;
 
+#if lnx_bullet
+import leenkx.trait.physics.bullet.PhysicsWorld.DebugDrawMode;
+
 import bullet.Bt.Vector3;
 
 import kha.FastFloat;
@@ -18,14 +21,19 @@ class DebugDrawHelper {
 	static inline var contactPointNormalColor = 0xffffffff;
 	static inline var contactPointDrawLifetime = true;
 
+	final rayCastColor: Vec4 = new Vec4(0.0, 1.0, 0.0);
+	final rayCastHitColor: Vec4 = new Vec4(1.0, 0.0, 0.0);
+	final rayCastHitPointColor: Vec4 = new Vec4(1.0, 1.0, 0.0);
+
 	final physicsWorld: PhysicsWorld;
 	final lines: Array<LineData> = [];
 	final texts: Array<TextData> = [];
 	var font: kha.Font = null;
 
-	var debugMode: PhysicsWorld.DebugDrawMode = NoDebug;
+	var rayCasts:Array<TRayCastData> = [];
+	var debugDrawMode: DebugDrawMode = NoDebug;
 
-	public function new(physicsWorld: PhysicsWorld) {
+	public function new(physicsWorld: PhysicsWorld, debugDrawMode: DebugDrawMode)
 		this.physicsWorld = physicsWorld;
 
 		#if lnx_ui
@@ -35,6 +43,11 @@ class DebugDrawHelper {
 		#end
 
 		iron.App.notifyOnRender2D(onRender);
+		if (debugDrawMode & DrawRayCast != 0) {
+			iron.App.notifyOnUpdate(function () {
+				rayCasts.resize(0);
+			});
+		}
 	}
 
 	public function drawLine(from: bullet.Bt.Vector3, to: bullet.Bt.Vector3, color: bullet.Bt.Vector3) {
@@ -63,25 +76,6 @@ class DebugDrawHelper {
 		}
 	}
 
-	// Draws raycast in its own function because 
-	// something is conflicting with the btVector3 and JS pointer wrapping
-	public function drawRayCast(fx:FastFloat, fy:FastFloat, fz:FastFloat, tx:FastFloat, ty:FastFloat, tz:FastFloat, r:FastFloat, g:FastFloat, b:FastFloat) {
-		final fromScreenSpace = worldToScreenFast(new Vec4(fx, fy, fz, 1.0));
-		final toScreenSpace = worldToScreenFast(new Vec4(tx, ty, tz, 1.0));
-
-		// TO DO: May still go off screen sides.
-		if (fromScreenSpace.w == 1 || toScreenSpace.w == 1) {
-			final color = kha.Color.fromFloats(r, g, b, 1.0);
-			lines.push({
-				fromX: fromScreenSpace.x,
-				fromY: fromScreenSpace.y,
-				toX: toScreenSpace.x,
-				toY: toScreenSpace.y,
-				color: color
-			});
-		}
-	}
-
 	public function drawContactPoint(pointOnB: Vector3, normalOnB: Vector3, distance: kha.FastFloat, lifeTime: Int, color: Vector3) {
 		#if js
 			pointOnB = js.Syntax.code("Ammo.wrapPointer({0}, Ammo.btVector3)", pointOnB);
@@ -126,7 +120,62 @@ class DebugDrawHelper {
 					x: contactPointScreenSpace.x,
 					y: contactPointScreenSpace.y,
 					color: color,
-					text: Std.string(lifeTime),
+					text: Std.string(lifeTime), // lifeTime: number of frames the contact point existed
+				});
+			}
+		}
+	}
+
+	public function rayCast(rayCastData:TRayCastData) {
+		rayCasts.push(rayCastData);
+	}
+
+	function drawRayCast(f: Vec4, t: Vec4, hit: Bool) {
+		final from = worldToScreenFast(f.clone());
+		final to = worldToScreenFast(t.clone());
+		var c: kha.Color;
+
+		if (from.w == 1 && to.w == 1) {
+			if (hit) c = kha.Color.fromFloats(rayCastHitColor.x, rayCastHitColor.y, rayCastHitColor.z);
+			else c = kha.Color.fromFloats(rayCastColor.x, rayCastColor.y, rayCastColor.z);
+
+			lines.push({
+				fromX: from.x,
+				fromY: from.y,
+				toX: to.x,
+				toY: to.y,
+				color: c
+			});
+		}
+	}
+
+	function drawHitPoint(hp: Vec4) {
+		final hitPoint = worldToScreenFast(hp.clone());
+		final c = kha.Color.fromFloats(rayCastHitPointColor.x, rayCastHitPointColor.y, rayCastHitPointColor.z);
+
+		if (hitPoint.w == 1) {
+			lines.push({
+				fromX: hitPoint.x - contactPointSizePx,
+				fromY: hitPoint.y - contactPointSizePx,
+				toX: hitPoint.x + contactPointSizePx,
+				toY: hitPoint.y + contactPointSizePx,
+				color: c
+			});
+
+			lines.push({
+				fromX: hitPoint.x - contactPointSizePx,
+				fromY: hitPoint.y + contactPointSizePx,
+				toX: hitPoint.x + contactPointSizePx,
+				toY: hitPoint.y - contactPointSizePx,
+				color: c
+			});
+
+			if (font != null) {
+				texts.push({
+					x: hitPoint.x,
+					y: hitPoint.y,
+					color: c,
+					text: 'RAYCAST HIT'
 				});
 			}
 		}
@@ -155,13 +204,13 @@ class DebugDrawHelper {
 		});
 	}
 
-	public function setDebugMode(debugMode: PhysicsWorld.DebugDrawMode) {
+	public function setDebugMode(debugDrawMode: DebugDrawMode) {
-		this.debugMode = debugMode;
+		this.debugDrawMode = debugDrawMode;
 	}
 
-	public function getDebugMode(): PhysicsWorld.DebugDrawMode {
+	public function getDebugMode(): DebugDrawMode {
 		#if js
-			return debugMode;
+			return DebugDrawMode;
 		#elseif hl
 			return physicsWorld.getDebugDrawMode();
 		#else
@@ -170,7 +219,7 @@ class DebugDrawHelper {
 	}
 
 	function onRender(g: kha.graphics2.Graphics) {
-		if (getDebugMode() == NoDebug && !physicsWorld.drawRaycasts) {
+		if (getDebugMode() == NoDebug) {
 			return;
 		}
 
@@ -179,7 +228,9 @@ class DebugDrawHelper {
 		// will cause Bullet to call the btIDebugDraw callbacks), but this way
 		// we can ensure that--within a frame--the function will not be called
 		// before some user-specific physics update, which would result in a
-		// one-frame drawing delay...
+		// one-frame drawing delay... Ideally we would ensure that debugDrawWorld()
+		// is called when all other (late) update callbacks are already executed...
+		
 		physicsWorld.world.debugDrawWorld();
 
 		g.opacity = 1.0;
@@ -199,6 +250,17 @@ class DebugDrawHelper {
 			}
 			texts.resize(0);
 		}
+		
+		if (debugDrawMode & DrawRayCast != 0) {
+			for (rayCastData in rayCasts) {
+				if (rayCastData.hasHit) {
+					drawRayCast(rayCastData.from, rayCastData.hitPoint, true);
+					drawHitPoint(rayCastData.hitPoint);
+				} else {
+					drawRayCast(rayCastData.from, rayCastData.to, false);
+				}
+			}
+		}
 	}
 
 	/**
@@ -241,3 +303,12 @@ class TextData {
 	public var color: kha.Color;
 	public var text: String;
 }
+
+@:structInit
+typedef TRayCastData = {
+	var from: Vec4;
+	var to: Vec4;
+	var hasHit: Bool;
+	@:optional var hitPoint: Vec4;
+	@:optional var hitNormal: Vec4;
+}