316 lines
8.7 KiB
Haxe
316 lines
8.7 KiB
Haxe
package leenkx.trait.physics.bullet;
|
|
|
|
#if lnx_bullet
|
|
import leenkx.trait.physics.bullet.PhysicsWorld.DebugDrawMode;
|
|
|
|
import bullet.Bt.Vector3;
|
|
|
|
import kha.FastFloat;
|
|
import kha.System;
|
|
|
|
import iron.math.Vec4;
|
|
|
|
#if lnx_ui
|
|
import leenkx.ui.Canvas;
|
|
#end
|
|
|
|
using StringTools;
|
|
|
|
class DebugDrawHelper {
|
|
static inline var contactPointSizePx = 4;
|
|
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 rayCasts:Array<TRayCastData> = [];
|
|
var debugDrawMode: DebugDrawMode = NoDebug;
|
|
|
|
public function new(physicsWorld: PhysicsWorld, debugDrawMode: DebugDrawMode) {
|
|
this.physicsWorld = physicsWorld;
|
|
|
|
#if lnx_ui
|
|
iron.data.Data.getFont(Canvas.defaultFontName, function(defaultFont: kha.Font) {
|
|
font = defaultFont;
|
|
});
|
|
#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) {
|
|
#if js
|
|
// https://github.com/InfiniteLee/ammo-debug-drawer/pull/1/files
|
|
// https://emscripten.org/docs/porting/connecting_cpp_and_javascript/WebIDL-Binder.html#pointers-and-comparisons
|
|
from = js.Syntax.code("Ammo.wrapPointer({0}, Ammo.btVector3)", from);
|
|
to = js.Syntax.code("Ammo.wrapPointer({0}, Ammo.btVector3)", to);
|
|
color = js.Syntax.code("Ammo.wrapPointer({0}, Ammo.btVector3)", color);
|
|
#end
|
|
|
|
final fromScreenSpace = worldToScreenFast(new Vec4(from.x(), from.y(), from.z(), 1.0));
|
|
final toScreenSpace = worldToScreenFast(new Vec4(to.x(), to.y(), to.z(), 1.0));
|
|
|
|
// investigate how to clamp lines to clip space borders
|
|
// If at least one point is within the Z clip space (w==1), attempt to draw.
|
|
// Note: This is not full clipping, line may still go off screen sides.
|
|
if (fromScreenSpace.w == 1 || toScreenSpace.w == 1) {
|
|
lines.push({
|
|
fromX: fromScreenSpace.x,
|
|
fromY: fromScreenSpace.y,
|
|
toX: toScreenSpace.x,
|
|
toY: toScreenSpace.y,
|
|
color: kha.Color.fromFloats(color.x(), color.y(), color.z(), 1.0)
|
|
});
|
|
}
|
|
}
|
|
|
|
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);
|
|
normalOnB = js.Syntax.code("Ammo.wrapPointer({0}, Ammo.btVector3)", normalOnB);
|
|
color = js.Syntax.code("Ammo.wrapPointer({0}, Ammo.btVector3)", color);
|
|
#end
|
|
|
|
final contactPointScreenSpace = worldToScreenFast(new Vec4(pointOnB.x(), pointOnB.y(), pointOnB.z(), 1.0));
|
|
final toScreenSpace = worldToScreenFast(new Vec4(pointOnB.x() + normalOnB.x() * distance, pointOnB.y() + normalOnB.y() * distance, pointOnB.z() + normalOnB.z() * distance, 1.0));
|
|
|
|
if (contactPointScreenSpace.w == 1) {
|
|
final color = kha.Color.fromFloats(color.x(), color.y(), color.z(), 1.0);
|
|
|
|
lines.push({
|
|
fromX: contactPointScreenSpace.x - contactPointSizePx,
|
|
fromY: contactPointScreenSpace.y - contactPointSizePx,
|
|
toX: contactPointScreenSpace.x + contactPointSizePx,
|
|
toY: contactPointScreenSpace.y + contactPointSizePx,
|
|
color: color
|
|
});
|
|
|
|
lines.push({
|
|
fromX: contactPointScreenSpace.x - contactPointSizePx,
|
|
fromY: contactPointScreenSpace.y + contactPointSizePx,
|
|
toX: contactPointScreenSpace.x + contactPointSizePx,
|
|
toY: contactPointScreenSpace.y - contactPointSizePx,
|
|
color: color
|
|
});
|
|
|
|
if (toScreenSpace.w == 1) {
|
|
lines.push({
|
|
fromX: contactPointScreenSpace.x,
|
|
fromY: contactPointScreenSpace.y,
|
|
toX: toScreenSpace.x,
|
|
toY: toScreenSpace.y,
|
|
color: contactPointNormalColor
|
|
});
|
|
}
|
|
|
|
if (contactPointDrawLifetime && font != null) {
|
|
texts.push({
|
|
x: contactPointScreenSpace.x,
|
|
y: contactPointScreenSpace.y,
|
|
color: color,
|
|
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'
|
|
});
|
|
}
|
|
}
|
|
}
|
|
|
|
public function reportErrorWarning(warningString: bullet.Bt.BulletString) {
|
|
trace(warningString.toHaxeString().trim());
|
|
}
|
|
|
|
public function draw3dText(location: Vector3, textString: bullet.Bt.BulletString) {
|
|
if (font == null) {
|
|
return;
|
|
}
|
|
|
|
#if js
|
|
location = js.Syntax.code("Ammo.wrapPointer({0}, Ammo.btVector3)", location);
|
|
#end
|
|
|
|
final locationScreenSpace = worldToScreenFast(new Vec4(location.x(), location.y(), location.z(), 1.0));
|
|
|
|
texts.push({
|
|
x: locationScreenSpace.x,
|
|
y: locationScreenSpace.y,
|
|
color: kha.Color.fromFloats(0.0, 0.0, 0.0, 1.0),
|
|
text: textString.toHaxeString()
|
|
});
|
|
}
|
|
|
|
public function setDebugMode(debugDrawMode: DebugDrawMode) {
|
|
this.debugDrawMode = debugDrawMode;
|
|
}
|
|
|
|
public function getDebugMode(): DebugDrawMode {
|
|
#if js
|
|
return DebugDrawMode;
|
|
#elseif hl
|
|
return physicsWorld.getDebugDrawMode();
|
|
#else
|
|
return NoDebug;
|
|
#end
|
|
}
|
|
|
|
function onRender(g: kha.graphics2.Graphics) {
|
|
if (getDebugMode() == NoDebug) {
|
|
return;
|
|
}
|
|
|
|
// It might be a bit unusual to call this method in a render callback
|
|
// instead of the update loop (after all it doesn't draw anything but
|
|
// 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... Ideally we would ensure that debugDrawWorld()
|
|
// is called when all other (late) update callbacks are already executed...
|
|
|
|
physicsWorld.world.debugDrawWorld();
|
|
|
|
g.opacity = 1.0;
|
|
|
|
for (line in lines) {
|
|
g.color = line.color;
|
|
g.drawLine(line.fromX, line.fromY, line.toX, line.toY, 1.0);
|
|
}
|
|
lines.resize(0);
|
|
|
|
if (font != null) {
|
|
g.font = font;
|
|
g.fontSize = 12;
|
|
for (text in texts) {
|
|
g.color = text.color;
|
|
g.drawString(text.text, text.x, text.y);
|
|
}
|
|
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);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
Transform a world coordinate vector into screen space and store the
|
|
result in the input vector's x and y coordinates. The w coordinate is
|
|
set to 0 if the input vector is outside the active camera's far and near
|
|
planes, and 1 otherwise.
|
|
**/
|
|
inline function worldToScreenFast(loc: Vec4): Vec4 {
|
|
final cam = iron.Scene.active.camera;
|
|
loc.w = 1.0;
|
|
loc.applyproj(cam.VP);
|
|
|
|
if (loc.z < -1 || loc.z > 1) {
|
|
loc.w = 0.0;
|
|
}
|
|
else {
|
|
loc.x = (loc.x + 1) * 0.5 * System.windowWidth();
|
|
loc.y = (1 - loc.y) * 0.5 * System.windowHeight();
|
|
loc.w = 1.0;
|
|
}
|
|
|
|
return loc;
|
|
}
|
|
}
|
|
|
|
@:structInit
|
|
class LineData {
|
|
public var fromX: FastFloat;
|
|
public var fromY: FastFloat;
|
|
public var toX: FastFloat;
|
|
public var toY: FastFloat;
|
|
public var color: kha.Color;
|
|
}
|
|
|
|
@:structInit
|
|
class TextData {
|
|
public var x: FastFloat;
|
|
public var y: FastFloat;
|
|
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;
|
|
}
|
|
|
|
#end |