forked from LeenkxTeam/LNXSDK
529 lines
14 KiB
Haxe
529 lines
14 KiB
Haxe
package leenkx.trait.physics.jolt;
|
|
|
|
#if lnx_jolt
|
|
|
|
import iron.Trait;
|
|
import iron.math.Vec4;
|
|
import iron.object.MeshObject;
|
|
import iron.data.MeshData;
|
|
import iron.data.SceneFormat;
|
|
import iron.system.Time;
|
|
import kha.arrays.ByteArray;
|
|
|
|
@:enum abstract SoftShape(Int) from Int {
|
|
var Cloth = 0;
|
|
var Volume = 1;
|
|
}
|
|
|
|
// Soft body particle for spring-mass simulation
|
|
class SoftParticle {
|
|
public var position:Vec4;
|
|
public var velocity:Vec4;
|
|
public var acceleration:Vec4;
|
|
public var invMass:Float;
|
|
public var pinned:Bool;
|
|
|
|
public function new(x:Float, y:Float, z:Float, mass:Float) {
|
|
position = new Vec4(x, y, z);
|
|
velocity = new Vec4(0, 0, 0);
|
|
acceleration = new Vec4(0, 0, 0);
|
|
invMass = mass > 0 ? 1.0 / mass : 0;
|
|
pinned = false;
|
|
}
|
|
|
|
public function applyForce(force:Vec4) {
|
|
acceleration.x += force.x * invMass;
|
|
acceleration.y += force.y * invMass;
|
|
acceleration.z += force.z * invMass;
|
|
}
|
|
|
|
public function integrate(dt:Float) {
|
|
if (pinned || invMass == 0)
|
|
return;
|
|
|
|
velocity.x += acceleration.x * dt;
|
|
velocity.y += acceleration.y * dt;
|
|
velocity.z += acceleration.z * dt;
|
|
|
|
// Damping
|
|
velocity.x *= 0.99;
|
|
velocity.y *= 0.99;
|
|
velocity.z *= 0.99;
|
|
|
|
position.x += velocity.x * dt;
|
|
position.y += velocity.y * dt;
|
|
position.z += velocity.z * dt;
|
|
|
|
acceleration.set(0, 0, 0);
|
|
}
|
|
}
|
|
|
|
// Spring constraint between particles
|
|
class SoftSpring {
|
|
public var p1:Int;
|
|
public var p2:Int;
|
|
public var restLength:Float;
|
|
public var stiffness:Float;
|
|
|
|
public function new(p1:Int, p2:Int, restLength:Float, stiffness:Float) {
|
|
this.p1 = p1;
|
|
this.p2 = p2;
|
|
this.restLength = restLength;
|
|
this.stiffness = stiffness;
|
|
}
|
|
}
|
|
|
|
class SoftBody extends Trait {
|
|
|
|
static var physics:PhysicsWorld = null;
|
|
|
|
public var ready = false;
|
|
var shape:SoftShape;
|
|
var bend:Float;
|
|
var mass:Float;
|
|
var margin:Float;
|
|
|
|
public var vertOffsetX = 0.0;
|
|
public var vertOffsetY = 0.0;
|
|
public var vertOffsetZ = 0.0;
|
|
|
|
// Spring-mass simulation
|
|
var particles:Array<SoftParticle> = [];
|
|
var springs:Array<SoftSpring> = [];
|
|
var gravity:Vec4;
|
|
var iterations = 3;
|
|
|
|
// Mesh data for vertex updates
|
|
var meshObject:MeshObject;
|
|
|
|
// Vertex deduplication: maps unique vertex index → list of raw vertex indices
|
|
// Meshes split vertices for normals/UVs, but soft body particles must be unique
|
|
var vertexIndexMap:Map<Int, Array<Int>>;
|
|
|
|
public function new(shape = SoftShape.Cloth, bend = 0.5, mass = 1.0, margin = 0.04) {
|
|
super();
|
|
this.shape = shape;
|
|
this.bend = bend;
|
|
this.mass = mass;
|
|
this.margin = margin;
|
|
|
|
notifyOnInit(init);
|
|
}
|
|
|
|
function init() {
|
|
var mo = cast(object, MeshObject);
|
|
new MeshData(mo.data.raw, function(data) {
|
|
mo.setData(data);
|
|
initSoftBody();
|
|
});
|
|
}
|
|
|
|
function retryInit() {
|
|
iron.App.removeUpdate(retryInit);
|
|
initSoftBody();
|
|
}
|
|
|
|
function initSoftBody() {
|
|
if (ready)
|
|
return;
|
|
|
|
if (PhysicsWorld.active == null || !PhysicsWorld.active.physicsReady) {
|
|
iron.App.notifyOnUpdate(retryInit);
|
|
return;
|
|
}
|
|
|
|
ready = true;
|
|
|
|
if (physics == null)
|
|
physics = PhysicsWorld.active;
|
|
|
|
meshObject = cast(object, MeshObject);
|
|
meshObject.frustumCulling = false;
|
|
var geom = meshObject.data.geom;
|
|
var rawData = meshObject.data.raw;
|
|
|
|
// Get gravity from physics world
|
|
var g = physics.getGravity();
|
|
gravity = new Vec4(g.x, g.y, g.z);
|
|
|
|
// Parented soft body - clear parent location
|
|
if (object.parent != null && object.parent.name != "") {
|
|
object.transform.loc.x += object.parent.transform.worldx();
|
|
object.transform.loc.y += object.parent.transform.worldy();
|
|
object.transform.loc.z += object.parent.transform.worldz();
|
|
object.transform.localOnly = true;
|
|
object.transform.buildMatrix();
|
|
}
|
|
|
|
// Build vertex deduplication map from vertex_map (matching Bullet SoftBody pattern)
|
|
// vertex_map[rawVertexBufferIdx] = uniqueVertexIdx
|
|
// vertex_map.length = number of raw vertices in vertex buffer (NOT same as geom.indices length)
|
|
// vertexIndexMap: uniqueVertexIdx → [rawVertexBufferIdx, ...]
|
|
vertexIndexMap = new Map();
|
|
var hasVertexMap = false;
|
|
for (ind in rawData.index_arrays) {
|
|
if (ind.vertex_map != null) {
|
|
hasVertexMap = true;
|
|
for (rawIdx in 0...ind.vertex_map.length) {
|
|
var uniqueIdx = ind.vertex_map[rawIdx];
|
|
var mapping = vertexIndexMap.get(uniqueIdx);
|
|
if (mapping == null) {
|
|
vertexIndexMap.set(uniqueIdx, [rawIdx]);
|
|
} else {
|
|
if (!mapping.contains(rawIdx))
|
|
mapping.push(rawIdx);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
var positions = geom.positions.values;
|
|
var scalePos = meshObject.data.scalePos;
|
|
|
|
if (hasVertexMap) {
|
|
// Create particles only for unique vertices
|
|
var numUnique = 0;
|
|
for (_ in vertexIndexMap.keys())
|
|
numUnique++;
|
|
|
|
for (key in 0...numUnique) {
|
|
var rawIndices = vertexIndexMap.get(key);
|
|
if (rawIndices == null || rawIndices.length == 0)
|
|
continue;
|
|
var ri = rawIndices[0];
|
|
var x = (positions[ri * 4] / 32767) * scalePos;
|
|
var y = (positions[ri * 4 + 1] / 32767) * scalePos;
|
|
var z = (positions[ri * 4 + 2] / 32767) * scalePos;
|
|
|
|
// Apply object rotation and scale
|
|
var vt = new Vec4(x, y, z);
|
|
vt.applyQuat(object.transform.rot);
|
|
vt.x *= object.transform.scale.x;
|
|
vt.y *= object.transform.scale.y;
|
|
vt.z *= object.transform.scale.z;
|
|
vt.addf(object.transform.worldx(), object.transform.worldy(), object.transform.worldz());
|
|
|
|
var particle = new SoftParticle(vt.x, vt.y, vt.z, mass / numUnique);
|
|
particles.push(particle);
|
|
}
|
|
|
|
// Create springs from triangle connectivity
|
|
// geom.indices[ia] = triangle index list of raw vertex buffer indices
|
|
// vertex_map[rawIdx] = unique vertex index for that raw vertex
|
|
var createdSprings:Map<String, Bool> = new Map();
|
|
var indexArrayIdx = 0;
|
|
for (ind in rawData.index_arrays) {
|
|
if (ind.vertex_map == null) {
|
|
indexArrayIdx++;
|
|
continue;
|
|
}
|
|
var indexArray = geom.indices[indexArrayIdx];
|
|
var numTris = Std.int(indexArray.length / 3);
|
|
for (i in 0...numTris) {
|
|
var r0 = indexArray[i * 3];
|
|
var r1 = indexArray[i * 3 + 1];
|
|
var r2 = indexArray[i * 3 + 2];
|
|
// Map raw vertex buffer indices → unique vertex indices
|
|
var u0 = ind.vertex_map[r0];
|
|
var u1 = ind.vertex_map[r1];
|
|
var u2 = ind.vertex_map[r2];
|
|
|
|
createSpring(u0, u1, createdSprings);
|
|
createSpring(u1, u2, createdSprings);
|
|
createSpring(u2, u0, createdSprings);
|
|
}
|
|
indexArrayIdx++;
|
|
}
|
|
} else {
|
|
// Fallback: no vertex_map, treat each vertex as unique
|
|
var numVerts = Std.int(positions.length / 4);
|
|
vertexIndexMap = new Map();
|
|
for (i in 0...numVerts) {
|
|
vertexIndexMap.set(i, [i]);
|
|
|
|
var x = (positions[i * 4] / 32767) * scalePos;
|
|
var y = (positions[i * 4 + 1] / 32767) * scalePos;
|
|
var z = (positions[i * 4 + 2] / 32767) * scalePos;
|
|
|
|
var vt = new Vec4(x, y, z);
|
|
vt.applyQuat(object.transform.rot);
|
|
vt.x *= object.transform.scale.x;
|
|
vt.y *= object.transform.scale.y;
|
|
vt.z *= object.transform.scale.z;
|
|
vt.addf(object.transform.worldx(), object.transform.worldy(), object.transform.worldz());
|
|
|
|
var particle = new SoftParticle(vt.x, vt.y, vt.z, mass / numVerts);
|
|
particles.push(particle);
|
|
}
|
|
|
|
var createdSprings:Map<String, Bool> = new Map();
|
|
for (indexArray in geom.indices) {
|
|
var numTris = Std.int(indexArray.length / 3);
|
|
for (i in 0...numTris) {
|
|
createSpring(indexArray[i * 3], indexArray[i * 3 + 1], createdSprings);
|
|
createSpring(indexArray[i * 3 + 1], indexArray[i * 3 + 2], createdSprings);
|
|
createSpring(indexArray[i * 3 + 2], indexArray[i * 3], createdSprings);
|
|
}
|
|
}
|
|
}
|
|
|
|
// Pin top edge vertices for cloth simulation
|
|
if (shape == SoftShape.Cloth) {
|
|
var minX = Math.POSITIVE_INFINITY, maxX = Math.NEGATIVE_INFINITY;
|
|
var minY = Math.POSITIVE_INFINITY, maxY = Math.NEGATIVE_INFINITY;
|
|
var minZ = Math.POSITIVE_INFINITY, maxZ = Math.NEGATIVE_INFINITY;
|
|
for (p in particles) {
|
|
if (p.position.x < minX) minX = p.position.x;
|
|
if (p.position.x > maxX) maxX = p.position.x;
|
|
if (p.position.y < minY) minY = p.position.y;
|
|
if (p.position.y > maxY) maxY = p.position.y;
|
|
if (p.position.z < minZ) minZ = p.position.z;
|
|
if (p.position.z > maxZ) maxZ = p.position.z;
|
|
}
|
|
var extX = maxX - minX;
|
|
var extY = maxY - minY;
|
|
var extZ = maxZ - minZ;
|
|
|
|
if (extZ > 0.01) {
|
|
var threshold = maxZ - extZ * 0.05;
|
|
for (p in particles) {
|
|
if (p.position.z >= threshold)
|
|
p.pinned = true;
|
|
}
|
|
} else if (extY > 0.01) {
|
|
var threshold = maxY - extY * 0.05;
|
|
for (p in particles) {
|
|
if (p.position.y >= threshold)
|
|
p.pinned = true;
|
|
}
|
|
} else if (extX > 0.01) {
|
|
var threshold = maxX - extX * 0.05;
|
|
for (p in particles) {
|
|
if (p.position.x >= threshold)
|
|
p.pinned = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
notifyOnRemove(removeFromWorld);
|
|
notifyOnUpdate(update);
|
|
}
|
|
|
|
function createSpring(i0:Int, i1:Int, createdSprings:Map<String, Bool>) {
|
|
if (i0 == i1)
|
|
return;
|
|
var key = i0 < i1 ? '${i0}_${i1}' : '${i1}_${i0}';
|
|
if (createdSprings.exists(key))
|
|
return;
|
|
createdSprings.set(key, true);
|
|
|
|
if (i0 >= particles.length || i1 >= particles.length)
|
|
return;
|
|
|
|
var p0 = particles[i0];
|
|
var p1 = particles[i1];
|
|
var dx = p1.position.x - p0.position.x;
|
|
var dy = p1.position.y - p0.position.y;
|
|
var dz = p1.position.z - p0.position.z;
|
|
var restLength = Math.sqrt(dx * dx + dy * dy + dz * dz);
|
|
|
|
if (restLength < 0.0001)
|
|
return;
|
|
|
|
springs.push(new SoftSpring(i0, i1, restLength, 1.0 - bend));
|
|
}
|
|
|
|
function update() {
|
|
var dt = Time.delta;
|
|
if (dt <= 0 || dt > 0.1)
|
|
return;
|
|
|
|
// Apply gravity as acceleration (m/s^2, independent of mass)
|
|
for (p in particles) {
|
|
if (!p.pinned) {
|
|
p.acceleration.x += gravity.x;
|
|
p.acceleration.y += gravity.y;
|
|
p.acceleration.z += gravity.z;
|
|
}
|
|
}
|
|
|
|
// Integrate particles
|
|
for (p in particles) {
|
|
p.integrate(dt);
|
|
}
|
|
|
|
// Solve spring constraints
|
|
for (iter in 0...iterations) {
|
|
for (spring in springs) {
|
|
solveSpring(spring);
|
|
}
|
|
}
|
|
|
|
// Ground collision
|
|
for (p in particles) {
|
|
if (p.position.z < 0) {
|
|
p.position.z = 0;
|
|
p.velocity.z = 0;
|
|
}
|
|
}
|
|
|
|
// Update mesh vertices
|
|
updateMeshVertices();
|
|
}
|
|
|
|
function solveSpring(spring:SoftSpring) {
|
|
var p1 = particles[spring.p1];
|
|
var p2 = particles[spring.p2];
|
|
|
|
var dx = p2.position.x - p1.position.x;
|
|
var dy = p2.position.y - p1.position.y;
|
|
var dz = p2.position.z - p1.position.z;
|
|
var dist = Math.sqrt(dx * dx + dy * dy + dz * dz);
|
|
|
|
if (dist < 0.0001)
|
|
return;
|
|
|
|
var diff = (dist - spring.restLength) / dist;
|
|
var stiffness = spring.stiffness;
|
|
var correction = diff * stiffness * 0.5;
|
|
|
|
if (!p1.pinned && p1.invMass > 0) {
|
|
p1.position.x += dx * correction;
|
|
p1.position.y += dy * correction;
|
|
p1.position.z += dz * correction;
|
|
}
|
|
|
|
if (!p2.pinned && p2.invMass > 0) {
|
|
p2.position.x -= dx * correction;
|
|
p2.position.y -= dy * correction;
|
|
p2.position.z -= dz * correction;
|
|
}
|
|
}
|
|
|
|
function updateMeshVertices() {
|
|
var geom = meshObject.data.geom;
|
|
var numNodes = particles.length;
|
|
|
|
// Compute mean position (center of mass) for object placement
|
|
vertOffsetX = 0.0;
|
|
vertOffsetY = 0.0;
|
|
vertOffsetZ = 0.0;
|
|
for (p in particles) {
|
|
vertOffsetX += p.position.x;
|
|
vertOffsetY += p.position.y;
|
|
vertOffsetZ += p.position.z;
|
|
}
|
|
vertOffsetX /= numNodes;
|
|
vertOffsetY /= numNodes;
|
|
vertOffsetZ /= numNodes;
|
|
|
|
// Set object transform to center of mass
|
|
meshObject.transform.scale.set(1, 1, 1);
|
|
meshObject.transform.loc.set(vertOffsetX, vertOffsetY, vertOffsetZ);
|
|
meshObject.transform.rot.set(0, 0, 0, 1);
|
|
|
|
// Compute scalePos to fit all vertices
|
|
var scalePos = 1.0;
|
|
for (p in particles) {
|
|
var mx = Math.abs((p.position.x - vertOffsetX) * 2);
|
|
var my = Math.abs((p.position.y - vertOffsetY) * 2);
|
|
var mz = Math.abs((p.position.z - vertOffsetZ) * 2);
|
|
if (mx > scalePos) scalePos = mx;
|
|
if (my > scalePos) scalePos = my;
|
|
if (mz > scalePos) scalePos = mz;
|
|
}
|
|
|
|
meshObject.data.scalePos = scalePos;
|
|
meshObject.transform.scaleWorld = scalePos;
|
|
meshObject.transform.buildMatrix();
|
|
|
|
var invScalePos = 1.0 / scalePos;
|
|
|
|
// Lock vertex buffer(s) for GPU upload
|
|
#if lnx_deinterleaved
|
|
var v:ByteArray = geom.vertexBuffers[0].buffer.lock();
|
|
#else
|
|
var v:ByteArray = geom.vertexBuffer.lock();
|
|
var vbPos = geom.vertexBufferMap.get("pos");
|
|
var v2 = vbPos != null ? vbPos.lock() : null;
|
|
var l = geom.structLength;
|
|
#end
|
|
|
|
// Write each unique particle position to all its raw vertex copies
|
|
for (uniqueIdx in 0...numNodes) {
|
|
var p = particles[uniqueIdx];
|
|
var indices = vertexIndexMap.get(uniqueIdx);
|
|
if (indices == null)
|
|
continue;
|
|
|
|
var mx = p.position.x - vertOffsetX;
|
|
var my = p.position.y - vertOffsetY;
|
|
var mz = p.position.z - vertOffsetZ;
|
|
|
|
var sx = Std.int(mx * 32767 * invScalePos);
|
|
var sy = Std.int(my * 32767 * invScalePos);
|
|
var sz = Std.int(mz * 32767 * invScalePos);
|
|
|
|
for (idx in indices) {
|
|
#if lnx_deinterleaved
|
|
v.setInt16(idx * 8, sx);
|
|
v.setInt16(idx * 8 + 2, sy);
|
|
v.setInt16(idx * 8 + 4, sz);
|
|
#else
|
|
var vertIndex = idx * l * 2;
|
|
v.setInt16(vertIndex, sx);
|
|
v.setInt16(vertIndex + 2, sy);
|
|
v.setInt16(vertIndex + 4, sz);
|
|
if (v2 != null) {
|
|
v2.setInt16(idx * 8, sx);
|
|
v2.setInt16(idx * 8 + 2, sy);
|
|
v2.setInt16(idx * 8 + 4, sz);
|
|
}
|
|
#end
|
|
}
|
|
}
|
|
|
|
// Unlock triggers GPU upload
|
|
#if lnx_deinterleaved
|
|
geom.vertexBuffers[0].buffer.unlock();
|
|
#else
|
|
geom.vertexBuffer.unlock();
|
|
if (vbPos != null) vbPos.unlock();
|
|
#end
|
|
}
|
|
|
|
public function pinVertex(index:Int) {
|
|
if (index >= 0 && index < particles.length) {
|
|
particles[index].pinned = true;
|
|
}
|
|
}
|
|
|
|
public function unpinVertex(index:Int) {
|
|
if (index >= 0 && index < particles.length) {
|
|
particles[index].pinned = false;
|
|
}
|
|
}
|
|
|
|
public function applyForceToVertex(index:Int, force:Vec4) {
|
|
if (index >= 0 && index < particles.length) {
|
|
particles[index].applyForce(force);
|
|
}
|
|
}
|
|
|
|
public function applyWindForce(direction:Vec4, strength:Float) {
|
|
var wind = new Vec4(direction.x * strength, direction.y * strength, direction.z * strength);
|
|
for (p in particles) {
|
|
if (!p.pinned) {
|
|
p.applyForce(wind);
|
|
}
|
|
}
|
|
}
|
|
|
|
function removeFromWorld() {
|
|
particles = [];
|
|
springs = [];
|
|
}
|
|
}
|
|
|
|
#end
|