forked from LeenkxTeam/LNXSDK
377 lines
10 KiB
Haxe
377 lines
10 KiB
Haxe
package leenkx.trait.physics.jolt;
|
|
|
|
#if lnx_jolt
|
|
|
|
import kha.FastFloat;
|
|
import kha.System;
|
|
import iron.math.Vec4;
|
|
|
|
#if lnx_ui
|
|
import leenkx.ui.Canvas;
|
|
#end
|
|
|
|
using StringTools;
|
|
|
|
enum abstract DebugDrawMode(Int) from Int to Int {
|
|
var NoDebug = 0;
|
|
var DrawWireframe = 1;
|
|
var DrawAabb = 2;
|
|
var DrawContactPoints = 4;
|
|
var DrawConstraints = 8;
|
|
var DrawConstraintLimits = 16;
|
|
var DrawRayCast = 32;
|
|
var DrawAll = 63;
|
|
|
|
@:op(A | B) static function or(lhs:DebugDrawMode, rhs:DebugDrawMode):DebugDrawMode;
|
|
@:op(A & B) static function and(lhs:DebugDrawMode, rhs:DebugDrawMode):DebugDrawMode;
|
|
}
|
|
|
|
class DebugDrawHelper {
|
|
static inline var contactPointSizePx = 4;
|
|
static inline var contactPointNormalColor = 0xffffffff;
|
|
|
|
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 wireframeColor:Vec4 = new Vec4(0.0, 1.0, 0.0);
|
|
final aabbColor:Vec4 = new Vec4(1.0, 1.0, 0.0);
|
|
final constraintColor:Vec4 = new Vec4(0.0, 0.5, 1.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;
|
|
this.debugDrawMode = debugDrawMode;
|
|
|
|
#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.notifyOnFixedUpdate(function() {
|
|
rayCasts.resize(0);
|
|
});
|
|
}
|
|
}
|
|
|
|
public function drawLine(fromX:Float, fromY:Float, fromZ:Float, toX:Float, toY:Float, toZ:Float, r:Float, g:Float, b:Float) {
|
|
final fromScreenSpace = worldToScreenFast(new Vec4(fromX, fromY, fromZ, 1.0));
|
|
final toScreenSpace = worldToScreenFast(new Vec4(toX, toY, toZ, 1.0));
|
|
|
|
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(r, g, b, 1.0)
|
|
});
|
|
}
|
|
}
|
|
|
|
public function drawLineVec(from:Vec4, to:Vec4, color:Vec4) {
|
|
drawLine(from.x, from.y, from.z, to.x, to.y, to.z, color.x, color.y, color.z);
|
|
}
|
|
|
|
public function drawContactPoint(pointX:Float, pointY:Float, pointZ:Float, normalX:Float, normalY:Float, normalZ:Float, distance:Float, r:Float, g:Float, b:Float) {
|
|
final contactPointScreenSpace = worldToScreenFast(new Vec4(pointX, pointY, pointZ, 1.0));
|
|
final toScreenSpace = worldToScreenFast(new Vec4(pointX + normalX * distance, pointY + normalY * distance, pointZ + normalZ * distance, 1.0));
|
|
|
|
if (contactPointScreenSpace.w == 1) {
|
|
final color = kha.Color.fromFloats(r, g, b, 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
|
|
});
|
|
}
|
|
}
|
|
}
|
|
|
|
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 drawBox(center:Vec4, halfExtents:Vec4, color:Vec4) {
|
|
var c = center;
|
|
var h = halfExtents;
|
|
|
|
// Bottom face
|
|
drawLine(c.x - h.x, c.y - h.y, c.z - h.z, c.x + h.x, c.y - h.y, c.z - h.z, color.x, color.y, color.z);
|
|
drawLine(c.x + h.x, c.y - h.y, c.z - h.z, c.x + h.x, c.y + h.y, c.z - h.z, color.x, color.y, color.z);
|
|
drawLine(c.x + h.x, c.y + h.y, c.z - h.z, c.x - h.x, c.y + h.y, c.z - h.z, color.x, color.y, color.z);
|
|
drawLine(c.x - h.x, c.y + h.y, c.z - h.z, c.x - h.x, c.y - h.y, c.z - h.z, color.x, color.y, color.z);
|
|
|
|
// Top face
|
|
drawLine(c.x - h.x, c.y - h.y, c.z + h.z, c.x + h.x, c.y - h.y, c.z + h.z, color.x, color.y, color.z);
|
|
drawLine(c.x + h.x, c.y - h.y, c.z + h.z, c.x + h.x, c.y + h.y, c.z + h.z, color.x, color.y, color.z);
|
|
drawLine(c.x + h.x, c.y + h.y, c.z + h.z, c.x - h.x, c.y + h.y, c.z + h.z, color.x, color.y, color.z);
|
|
drawLine(c.x - h.x, c.y + h.y, c.z + h.z, c.x - h.x, c.y - h.y, c.z + h.z, color.x, color.y, color.z);
|
|
|
|
// Vertical edges
|
|
drawLine(c.x - h.x, c.y - h.y, c.z - h.z, c.x - h.x, c.y - h.y, c.z + h.z, color.x, color.y, color.z);
|
|
drawLine(c.x + h.x, c.y - h.y, c.z - h.z, c.x + h.x, c.y - h.y, c.z + h.z, color.x, color.y, color.z);
|
|
drawLine(c.x + h.x, c.y + h.y, c.z - h.z, c.x + h.x, c.y + h.y, c.z + h.z, color.x, color.y, color.z);
|
|
drawLine(c.x - h.x, c.y + h.y, c.z - h.z, c.x - h.x, c.y + h.y, c.z + h.z, color.x, color.y, color.z);
|
|
}
|
|
|
|
public function drawSphere(center:Vec4, radius:Float, color:Vec4) {
|
|
final segments = 16;
|
|
final step = Math.PI * 2 / segments;
|
|
|
|
// XY circle
|
|
for (i in 0...segments) {
|
|
var angle1 = i * step;
|
|
var angle2 = (i + 1) * step;
|
|
drawLine(
|
|
center.x + Math.cos(angle1) * radius, center.y + Math.sin(angle1) * radius, center.z,
|
|
center.x + Math.cos(angle2) * radius, center.y + Math.sin(angle2) * radius, center.z,
|
|
color.x, color.y, color.z
|
|
);
|
|
}
|
|
|
|
// XZ circle
|
|
for (i in 0...segments) {
|
|
var angle1 = i * step;
|
|
var angle2 = (i + 1) * step;
|
|
drawLine(
|
|
center.x + Math.cos(angle1) * radius, center.y, center.z + Math.sin(angle1) * radius,
|
|
center.x + Math.cos(angle2) * radius, center.y, center.z + Math.sin(angle2) * radius,
|
|
color.x, color.y, color.z
|
|
);
|
|
}
|
|
|
|
// YZ circle
|
|
for (i in 0...segments) {
|
|
var angle1 = i * step;
|
|
var angle2 = (i + 1) * step;
|
|
drawLine(
|
|
center.x, center.y + Math.cos(angle1) * radius, center.z + Math.sin(angle1) * radius,
|
|
center.x, center.y + Math.cos(angle2) * radius, center.z + Math.sin(angle2) * radius,
|
|
color.x, color.y, color.z
|
|
);
|
|
}
|
|
}
|
|
|
|
public function setDebugMode(debugDrawMode:DebugDrawMode) {
|
|
this.debugDrawMode = debugDrawMode;
|
|
}
|
|
|
|
public function getDebugMode():DebugDrawMode {
|
|
return debugDrawMode;
|
|
}
|
|
|
|
function drawBodyWireframe(body:RigidBody) {
|
|
if (body == null || body.object == null)
|
|
return;
|
|
|
|
var transform = body.object.transform;
|
|
var pos = transform.world.getLoc();
|
|
var dim = transform.dim;
|
|
var halfExtents = new Vec4(dim.x * 0.5, dim.y * 0.5, dim.z * 0.5);
|
|
|
|
drawBox(pos, halfExtents, wireframeColor);
|
|
}
|
|
|
|
function drawBodyAabb(body:RigidBody) {
|
|
if (body == null || body.object == null)
|
|
return;
|
|
|
|
var transform = body.object.transform;
|
|
var pos = transform.world.getLoc();
|
|
var dim = transform.dim;
|
|
var halfExtents = new Vec4(dim.x * 0.5, dim.y * 0.5, dim.z * 0.5);
|
|
|
|
drawBox(pos, halfExtents, aabbColor);
|
|
}
|
|
|
|
function drawConstraintDebug(constraint:PhysicsConstraint) {
|
|
if (constraint == null || constraint.body1 == null || constraint.body2 == null)
|
|
return;
|
|
|
|
var pos1 = constraint.body1.object.transform.world.getLoc();
|
|
var pos2 = constraint.body2.object.transform.world.getLoc();
|
|
|
|
drawLineVec(pos1, pos2, constraintColor);
|
|
}
|
|
|
|
function onRender(g:kha.graphics2.Graphics) {
|
|
if (getDebugMode() == NoDebug) {
|
|
return;
|
|
}
|
|
|
|
// Draw physics debug info
|
|
if (debugDrawMode & DrawWireframe != 0 || debugDrawMode & DrawAabb != 0) {
|
|
for (body in physicsWorld.rbMap) {
|
|
if (debugDrawMode & DrawWireframe != 0) {
|
|
drawBodyWireframe(body);
|
|
}
|
|
if (debugDrawMode & DrawAabb != 0) {
|
|
drawBodyAabb(body);
|
|
}
|
|
}
|
|
}
|
|
|
|
if (debugDrawMode & DrawConstraints != 0) {
|
|
for (constraint in physicsWorld.constraints) {
|
|
drawConstraintDebug(constraint);
|
|
}
|
|
}
|
|
|
|
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);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
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
|