forked from LeenkxTeam/LNXSDK
406 lines
12 KiB
Haxe
406 lines
12 KiB
Haxe
package leenkx.trait.physics.jolt;
|
|
|
|
#if lnx_jolt
|
|
|
|
import iron.Trait;
|
|
import iron.math.Vec4;
|
|
import iron.math.Quat;
|
|
import iron.math.Mat4;
|
|
import iron.object.Object;
|
|
|
|
@:enum abstract ConstraintType(Int) from Int to Int {
|
|
var Fixed = 0;
|
|
var Point = 1;
|
|
var Hinge = 2;
|
|
var Slider = 3;
|
|
var Piston = 4;
|
|
var Generic = 5;
|
|
var GenericSpring = 6;
|
|
var Distance = 7;
|
|
}
|
|
|
|
class PhysicsConstraint extends Trait {
|
|
public var id:Int;
|
|
public var physics:PhysicsWorld;
|
|
public var body1:RigidBody;
|
|
public var body2:RigidBody;
|
|
public var type:ConstraintType;
|
|
public var con:jolt.Jt.Constraint;
|
|
public var conReady:Bool = false;
|
|
public var disableCollisions:Bool;
|
|
|
|
var body1Obj:Object;
|
|
var body2Obj:Object;
|
|
var limits:Array<Float>;
|
|
var breakingThreshold:Float;
|
|
|
|
static var nextId = 0;
|
|
|
|
public function new(body1:Object, body2:Object, type:ConstraintType, disableCollisions:Bool = false, breakingThreshold:Float = 0.0,
|
|
limits:Array<Float> = null) {
|
|
super();
|
|
|
|
this.type = type;
|
|
this.disableCollisions = disableCollisions;
|
|
this.breakingThreshold = breakingThreshold;
|
|
this.limits = limits;
|
|
this.id = nextId++;
|
|
this.body1Obj = body1;
|
|
this.body2Obj = body2;
|
|
|
|
notifyOnInit(function() {
|
|
this.body1 = body1.getTrait(RigidBody);
|
|
this.body2 = body2.getTrait(RigidBody);
|
|
tryInit();
|
|
});
|
|
|
|
notifyOnRemove(removeFromWorld);
|
|
}
|
|
|
|
function tryInit() {
|
|
if (this.body1 != null && this.body1.ready && this.body2 != null && this.body2.ready) {
|
|
init();
|
|
} else if (this.body1 != null || this.body2 != null) {
|
|
// Bodies exist but not ready yet, retry next frame
|
|
iron.App.notifyOnUpdate(retryInit);
|
|
}
|
|
}
|
|
|
|
function retryInit() {
|
|
iron.App.removeUpdate(retryInit);
|
|
tryInit();
|
|
}
|
|
|
|
function init() {
|
|
physics = PhysicsWorld.active;
|
|
|
|
// Compute constraint frames in each body's local space (exactly matches Bullet approach)
|
|
var t = object.transform; // pivot object
|
|
var t1 = body1Obj.transform; // body1 object
|
|
var t2 = body2Obj.transform; // body2 object
|
|
|
|
// Frame In A: pivot transform in body1's local space
|
|
var frameT = t.world.clone();
|
|
var frameInA = t1.world.clone();
|
|
frameInA.getInverse(frameInA);
|
|
frameT.multmat(frameInA);
|
|
frameInA = frameT.clone();
|
|
|
|
// Frame In B: pivot transform in body2's local space
|
|
frameT = t.world.clone();
|
|
var frameInB = t2.world.clone();
|
|
frameInB.getInverse(frameInB);
|
|
frameT.multmat(frameInB);
|
|
frameInB = frameT.clone();
|
|
|
|
// Decompose frames to get local positions and orientations
|
|
var locA = new Vec4();
|
|
var rotA = new Quat();
|
|
var sclA = new Vec4();
|
|
frameInA.decompose(locA, rotA, sclA);
|
|
|
|
var locB = new Vec4();
|
|
var rotB = new Quat();
|
|
var sclB = new Vec4();
|
|
frameInB.decompose(locB, rotB, sclB);
|
|
|
|
// Extract local axes from each frame (normalized to remove scale)
|
|
var rightA = frameInA.right().normalize();
|
|
var upA = frameInA.up().normalize();
|
|
var rightB = frameInB.right().normalize();
|
|
var upB = frameInB.up().normalize();
|
|
|
|
// Create Jolt vectors for body1 local frame
|
|
var jPt1 = new jolt.Jt.RVec3(locA.x, locA.y, locA.z);
|
|
var jAxX1 = new jolt.Jt.Vec3(rightA.x, rightA.y, rightA.z);
|
|
var jAxY1 = new jolt.Jt.Vec3(upA.x, upA.y, upA.z);
|
|
|
|
// Create Jolt vectors for body2 local frame
|
|
var jPt2 = new jolt.Jt.RVec3(locB.x, locB.y, locB.z);
|
|
var jAxX2 = new jolt.Jt.Vec3(rightB.x, rightB.y, rightB.z);
|
|
var jAxY2 = new jolt.Jt.Vec3(upB.x, upB.y, upB.z);
|
|
|
|
switch (type) {
|
|
case Fixed:
|
|
var settings = new jolt.Jt.FixedConstraintSettings();
|
|
settings.mSpace = 0; // LocalToBodyCOM
|
|
settings.mAutoDetectPoint = false;
|
|
settings.mPoint1 = jPt1;
|
|
settings.mPoint2 = jPt2;
|
|
settings.mAxisX1 = jAxX1;
|
|
settings.mAxisY1 = jAxY1;
|
|
settings.mAxisX2 = jAxX2;
|
|
settings.mAxisY2 = jAxY2;
|
|
con = settings.Create(body1.body, body2.body);
|
|
#if hl settings.delete(); #end
|
|
|
|
case Point:
|
|
var settings = new jolt.Jt.PointConstraintSettings();
|
|
settings.mSpace = 0;
|
|
settings.mPoint1 = jPt1;
|
|
settings.mPoint2 = jPt2;
|
|
con = settings.Create(body1.body, body2.body);
|
|
#if hl settings.delete(); #end
|
|
|
|
case Hinge:
|
|
var settings = new jolt.Jt.HingeConstraintSettings();
|
|
settings.mSpace = 0;
|
|
settings.mPoint1 = jPt1;
|
|
settings.mPoint2 = jPt2;
|
|
settings.mHingeAxis1 = jAxY1;
|
|
settings.mHingeAxis2 = jAxY2;
|
|
settings.mNormalAxis1 = jAxX1;
|
|
settings.mNormalAxis2 = jAxX2;
|
|
if (limits != null && limits.length >= 3 && limits[0] != 0) {
|
|
settings.mLimitsMin = limits[1];
|
|
settings.mLimitsMax = limits[2];
|
|
}
|
|
con = settings.Create(body1.body, body2.body);
|
|
#if hl settings.delete(); #end
|
|
|
|
case Slider:
|
|
var settings = new jolt.Jt.SliderConstraintSettings();
|
|
settings.mSpace = 0;
|
|
settings.mAutoDetectPoint = false;
|
|
settings.mPoint1 = jPt1;
|
|
settings.mPoint2 = jPt2;
|
|
settings.mSliderAxis1 = jAxX1;
|
|
settings.mSliderAxis2 = jAxX2;
|
|
settings.mNormalAxis1 = jAxY1;
|
|
settings.mNormalAxis2 = jAxY2;
|
|
if (limits != null && limits.length >= 3 && limits[0] != 0) {
|
|
settings.mLimitsMin = limits[1];
|
|
settings.mLimitsMax = limits[2];
|
|
}
|
|
con = settings.Create(body1.body, body2.body);
|
|
#if hl settings.delete(); #end
|
|
|
|
case Distance:
|
|
var settings = new jolt.Jt.DistanceConstraintSettings();
|
|
settings.mSpace = 0;
|
|
settings.mPoint1 = jPt1;
|
|
settings.mPoint2 = jPt2;
|
|
if (limits != null && limits.length >= 2) {
|
|
settings.mMinDistance = limits[0];
|
|
settings.mMaxDistance = limits[1];
|
|
}
|
|
con = settings.Create(body1.body, body2.body);
|
|
#if hl settings.delete(); #end
|
|
|
|
case Piston:
|
|
var settings = new jolt.Jt.SliderConstraintSettings();
|
|
settings.mSpace = 0;
|
|
settings.mAutoDetectPoint = false;
|
|
settings.mPoint1 = jPt1;
|
|
settings.mPoint2 = jPt2;
|
|
settings.mSliderAxis1 = jAxY1;
|
|
settings.mSliderAxis2 = jAxY2;
|
|
settings.mNormalAxis1 = jAxX1;
|
|
settings.mNormalAxis2 = jAxX2;
|
|
if (limits != null && limits.length >= 3 && limits[0] != 0) {
|
|
settings.mLimitsMin = limits[1];
|
|
settings.mLimitsMax = limits[2];
|
|
}
|
|
con = settings.Create(body1.body, body2.body);
|
|
#if hl settings.delete(); #end
|
|
|
|
case Generic:
|
|
var settings = new jolt.Jt.SixDOFConstraintSettings();
|
|
settings.mSpace = 0;
|
|
settings.mPosition1 = jPt1;
|
|
settings.mPosition2 = jPt2;
|
|
settings.mAxisX1 = jAxX1;
|
|
settings.mAxisY1 = jAxY1;
|
|
settings.mAxisX2 = jAxX2;
|
|
settings.mAxisY2 = jAxY2;
|
|
if (limits != null) {
|
|
applySixDOFLimits(settings);
|
|
} else {
|
|
for (i in 0...6) settings.MakeFreeAxis(i);
|
|
}
|
|
con = settings.Create(body1.body, body2.body);
|
|
#if hl settings.delete(); #end
|
|
|
|
case GenericSpring:
|
|
var settings = new jolt.Jt.SixDOFConstraintSettings();
|
|
settings.mSpace = 0;
|
|
settings.mPosition1 = jPt1;
|
|
settings.mPosition2 = jPt2;
|
|
settings.mAxisX1 = jAxX1;
|
|
settings.mAxisY1 = jAxY1;
|
|
settings.mAxisX2 = jAxX2;
|
|
settings.mAxisY2 = jAxY2;
|
|
if (limits != null) {
|
|
applySixDOFLimits(settings);
|
|
} else {
|
|
for (i in 0...6) settings.MakeFreeAxis(i);
|
|
}
|
|
con = settings.Create(body1.body, body2.body);
|
|
#if hl settings.delete(); #end
|
|
|
|
default:
|
|
var settings = new jolt.Jt.FixedConstraintSettings();
|
|
settings.mSpace = 0;
|
|
settings.mAutoDetectPoint = false;
|
|
settings.mPoint1 = jPt1;
|
|
settings.mPoint2 = jPt2;
|
|
settings.mAxisX1 = jAxX1;
|
|
settings.mAxisY1 = jAxY1;
|
|
settings.mAxisX2 = jAxX2;
|
|
settings.mAxisY2 = jAxY2;
|
|
con = settings.Create(body1.body, body2.body);
|
|
#if hl settings.delete(); #end
|
|
}
|
|
|
|
// Clean up temporary Jolt objects
|
|
#if hl
|
|
jPt1.delete();
|
|
jPt2.delete();
|
|
jAxX1.delete();
|
|
jAxY1.delete();
|
|
jAxX2.delete();
|
|
jAxY2.delete();
|
|
#end
|
|
|
|
conReady = true;
|
|
physics.addPhysicsConstraint(this);
|
|
}
|
|
|
|
function applySixDOFLimits(settings:jolt.Jt.SixDOFConstraintSettings) {
|
|
// Linear X (limits[0..2]): limits[0]=enabled, limits[1]=lower, limits[2]=upper
|
|
if (limits.length > 2 && limits[0] != 0) {
|
|
if (limits[1] > limits[2])
|
|
settings.MakeFreeAxis(0);
|
|
else
|
|
settings.SetLimitedAxis(0, limits[1], limits[2]);
|
|
} else {
|
|
settings.MakeFreeAxis(0);
|
|
}
|
|
// Linear Y (limits[3..5])
|
|
if (limits.length > 5 && limits[3] != 0) {
|
|
if (limits[4] > limits[5])
|
|
settings.MakeFreeAxis(1);
|
|
else
|
|
settings.SetLimitedAxis(1, limits[4], limits[5]);
|
|
} else {
|
|
settings.MakeFreeAxis(1);
|
|
}
|
|
// Linear Z (limits[6..8])
|
|
if (limits.length > 8 && limits[6] != 0) {
|
|
if (limits[7] > limits[8])
|
|
settings.MakeFreeAxis(2);
|
|
else
|
|
settings.SetLimitedAxis(2, limits[7], limits[8]);
|
|
} else {
|
|
settings.MakeFreeAxis(2);
|
|
}
|
|
// Angular X (limits[9..11])
|
|
if (limits.length > 11 && limits[9] != 0) {
|
|
if (limits[10] > limits[11])
|
|
settings.MakeFreeAxis(3);
|
|
else
|
|
settings.SetLimitedAxis(3, limits[10], limits[11]);
|
|
} else {
|
|
settings.MakeFreeAxis(3);
|
|
}
|
|
// Angular Y (limits[12..14])
|
|
if (limits.length > 14 && limits[12] != 0) {
|
|
if (limits[13] > limits[14])
|
|
settings.MakeFreeAxis(4);
|
|
else
|
|
settings.SetLimitedAxis(4, limits[13], limits[14]);
|
|
} else {
|
|
settings.MakeFreeAxis(4);
|
|
}
|
|
// Angular Z (limits[15..17])
|
|
if (limits.length > 17 && limits[15] != 0) {
|
|
if (limits[16] > limits[17])
|
|
settings.MakeFreeAxis(5);
|
|
else
|
|
settings.SetLimitedAxis(5, limits[16], limits[17]);
|
|
} else {
|
|
settings.MakeFreeAxis(5);
|
|
}
|
|
}
|
|
|
|
function removeFromWorld() {
|
|
if (physics != null) {
|
|
physics.removePhysicsConstraint(this);
|
|
}
|
|
}
|
|
|
|
public function delete() {
|
|
conReady = false;
|
|
}
|
|
|
|
public function setEnabled(enabled:Bool) {
|
|
if (conReady) {
|
|
con.SetEnabled(enabled);
|
|
}
|
|
}
|
|
|
|
public function isEnabled():Bool {
|
|
return conReady ? con.GetEnabled() : false;
|
|
}
|
|
|
|
// Bullet-compatible limit setting methods
|
|
public function setHingeConstraintLimits(angLimit:Bool, lowerAngLimit:Float, upperAngLimit:Float) {
|
|
if (limits == null) limits = [for (i in 0...36) 0.0];
|
|
limits[0] = angLimit ? 1 : 0;
|
|
limits[1] = lowerAngLimit * (Math.PI / 180);
|
|
limits[2] = upperAngLimit * (Math.PI / 180);
|
|
}
|
|
|
|
public function setSliderConstraintLimits(linLimit:Bool, lowerLinLimit:Float, upperLinLimit:Float) {
|
|
if (limits == null) limits = [for (i in 0...36) 0.0];
|
|
limits[0] = linLimit ? 1 : 0;
|
|
limits[1] = lowerLinLimit;
|
|
limits[2] = upperLinLimit;
|
|
}
|
|
|
|
public function setPistonConstraintLimits(linLimit:Bool, lowerLinLimit:Float, upperLinLimit:Float, angLimit:Bool, lowerAngLimit:Float, upperAngLimit:Float) {
|
|
if (limits == null) limits = [for (i in 0...36) 0.0];
|
|
limits[0] = linLimit ? 1 : 0;
|
|
limits[1] = lowerLinLimit;
|
|
limits[2] = upperLinLimit;
|
|
limits[3] = angLimit ? 1 : 0;
|
|
limits[4] = lowerAngLimit * (Math.PI / 180);
|
|
limits[5] = upperAngLimit * (Math.PI / 180);
|
|
}
|
|
|
|
public function setGenericConstraintLimits(setLimit:Bool = false, lowerLimit:Float = 1.0, upperLimit:Float = -1.0, axis:ConstraintAxis = X, isAngular:Bool = false) {
|
|
if (limits == null) limits = [for (i in 0...36) 0.0];
|
|
var i = switch (axis) {
|
|
case X: 0;
|
|
case Y: 3;
|
|
case Z: 6;
|
|
};
|
|
var j = isAngular ? 9 : 0;
|
|
var radian = isAngular ? (Math.PI / 180) : 1;
|
|
limits[i + j] = setLimit ? 1 : 0;
|
|
limits[i + j + 1] = lowerLimit * radian;
|
|
limits[i + j + 2] = upperLimit * radian;
|
|
}
|
|
|
|
public function setSpringParams(setSpring:Bool = false, stiffness:Float = 10.0, damping:Float = 0.5, axis:ConstraintAxis = X, isAngular:Bool = false) {
|
|
if (limits == null) limits = [for (i in 0...36) 0.0];
|
|
var i = switch (axis) {
|
|
case X: 18;
|
|
case Y: 21;
|
|
case Z: 24;
|
|
};
|
|
var j = isAngular ? 9 : 0;
|
|
limits[i + j] = setSpring ? 1 : 0;
|
|
limits[i + j + 1] = stiffness;
|
|
limits[i + j + 2] = damping;
|
|
}
|
|
}
|
|
|
|
@:enum abstract ConstraintAxis(Int) from Int to Int {
|
|
var X = 0;
|
|
var Y = 1;
|
|
var Z = 2;
|
|
}
|
|
|
|
#end
|