diff --git a/src/Main.hx b/src/Main.hx index 95e852fc..b189ff7f 100644 --- a/src/Main.hx +++ b/src/Main.hx @@ -46,19 +46,19 @@ class Main extends hxd.App { var m1 = new PathedInteriorMarker(); m1.msToNext = 5; - m1.position = new Vector(5, 0, 0); + m1.position = new Vector(0, 0, 0); m1.smoothingType = ""; m1.rotation = new Quat(); var m2 = new PathedInteriorMarker(); m2.msToNext = 3; - m2.position = new Vector(5, 0, 5); + m2.position = new Vector(0, 0, 5); m2.smoothingType = ""; m2.rotation = new Quat(); var m3 = new PathedInteriorMarker(); m3.msToNext = 5; - m3.position = new Vector(5, 0, 0); + m3.position = new Vector(0, 0, 0); m3.smoothingType = ""; m3.rotation = new Quat(); @@ -86,14 +86,14 @@ class Main extends hxd.App { // s3d.camera. - var marble2 = new Marble(); - world.addMarble(marble2); - marble2.setPosition(0, 5, 5); + // var marble2 = new Marble(); + // world.addMarble(marble2); + // marble2.setPosition(5, 0, 5); var marble = new Marble(); marble.controllable = true; world.addMarble(marble); - marble.setPosition(6, 0, 5); + marble.setPosition(0, 0, 2); // marble.setPosition(-10, -5, 5); } diff --git a/src/Marble.hx b/src/Marble.hx index 00f55ae9..1b786586 100644 --- a/src/Marble.hx +++ b/src/Marble.hx @@ -1,5 +1,6 @@ package src; +import collision.Collision; import dif.math.Point3F; import dif.math.PlaneF; import collision.CollisionSurface; @@ -251,13 +252,20 @@ class Marble extends Object { dir.normalize(); var soFar = 0.0; for (k in 0...contacts.length) { - if (contacts[k].penetration < this._radius) { + if (contacts[k].contactDistance < this._radius) { var timeToSeparate = 0.1; - var dist = contacts[k].penetration; - var outVel = this.velocity.add(dir.multiply(soFar)).dot(contacts[k].normal); + var dist = this._radius - contacts[k].contactDistance; // contacts[k].penetration; + var normal = contacts[k].normal; + var unk = normal.multiply(soFar); + var tickle = this.velocity.sub(contacts[k].velocity); + var plop = unk.add(tickle); + var outVel = plop.dot(normal); + var cancan = timeToSeparate * outVel; - if (timeToSeparate * outVel < dist) { - soFar += (dist - outVel * timeToSeparate) / timeToSeparate / contacts[k].normal.dot(dir); + if (dist > cancan) { + var bla = contacts[k].normal; + var bFac = (dist - cancan) / timeToSeparate; + soFar += bFac / bla.dot(dir); } } } @@ -278,9 +286,9 @@ class Marble extends Object { var bestNormalForce = 0.0; for (i in 0...contacts.length) { if (contacts[i].collider == null) { - var normalForce = -contacts[i].normal.dot(A); - if (normalForce > bestNormalForce) { - bestNormalForce = normalForce; + contacts[i].normalForce = -contacts[i].normal.dot(A); + if (contacts[i].normalForce > bestNormalForce) { + bestNormalForce = contacts[i].normalForce; bestSurface = i; } } @@ -373,7 +381,7 @@ class Marble extends Object { function testMove(velocity:Vector, position:Vector, deltaT:Float, radius:Float, testPIs:Bool, collisionWorld:CollisionWorld) { var velLen = velocity.length(); if (velLen < 0.001) - return false; + return deltaT; var velocityDir = velocity.normalized(); @@ -411,7 +419,7 @@ class Marble extends Object { var surfaces = obj.octree.radiusSearch(localpos, expandedcollider.radius); for (surf in surfaces) { - var surface:CollisionSurface = cast obj; + var surface:CollisionSurface = cast surf; var i = 0; while (i < surface.indices.length) { @@ -655,27 +663,131 @@ class Marble extends Object { contacted = true; } - return true; + return finalT; } - function advancePhysics(m:Move, dt:Float, collisionWorld:CollisionWorld) { - this.findContacts(collisionWorld, dt); - var cmf = this.computeMoveForces(m); - var isCentered:Bool = cmf.result; - var aControl = cmf.aControl; - var desiredOmega = cmf.desiredOmega; - this.velocityCancel(isCentered, false); - var A = this.getExternalForces(m, dt); - var retf = this.applyContactForces(dt, m, isCentered, aControl, desiredOmega, A); - A = retf[0]; - var a = retf[1]; - this.velocity = this.velocity.add(A.multiply(dt)); - this.omega = this.omega.add(a.multiply(dt)); - this.velocityCancel(isCentered, true); - this._totalTime += dt; - if (contacts.length != 0) { - this._contactTime += dt; + function getIntersectionTime(dt:Float, velocity:Vector, pathedInteriors:Array, collisionWorld:CollisionWorld) { + var expandedcollider = new SphereCollisionEntity(cast this); + var position = this.getAbsPos().getPosition(); + expandedcollider.transform = Matrix.T(position.x, position.y, position.z); + expandedcollider.radius = this.getAbsPos().getPosition().distance(position) + _radius; + + var foundObjs = collisionWorld.radiusSearch(position, expandedcollider.radius); + + function toDifPoint(vec:Vector) { + return new Point3F(vec.x, vec.y, vec.z); } + + var intersectT = 10e8; + + for (obj in foundObjs) { + if (obj.velocity.length() > 0) { + var radius = _radius; + + var invMatrix = obj.transform.clone(); + invMatrix.invert(); + var localpos = position.clone(); + localpos.transform(invMatrix); + var surfaces = obj.octree.radiusSearch(localpos, radius * 1.1); + + var tform = obj.transform.clone(); + var velDir = obj.velocity.normalized(); + // tform.setPosition(tform.getPosition().add(velDir.multiply(_radius))); + tform.setPosition(tform.getPosition().add(obj.velocity.multiply(dt)).sub(velDir.multiply(_radius))); + + var contacts = []; + + for (surf in surfaces) { + var surface:CollisionSurface = cast surf; + + var i = 0; + while (i < surface.indices.length) { + var v0 = surface.points[surface.indices[i]].transformed(tform); + var v = surface.points[surface.indices[i + 1]].transformed(tform); + var v2 = surface.points[surface.indices[i + 2]].transformed(tform); + + var polyPlane = PlaneF.ThreePoints(toDifPoint(v0), toDifPoint(v), toDifPoint(v2)); + + var surfacenormal = surface.normals[surface.indices[i]].transformed3x3(obj.transform); + + var t = (-position.dot(surfacenormal) - polyPlane.d) / velocity.dot(surfacenormal); + + var pt = position.add(velocity.multiply(t)); + + if (Collision.PointInTriangle(pt, v0, v, v2)) { + if (t > 0 && t < intersectT) { + intersectT = t; + } + } + + i += 3; + } + } + } + } + + return intersectT; + } + + function advancePhysics(currentTime:Float, dt:Float, m:Move, collisionWorld:CollisionWorld, pathedInteriors:Array) { + var timeRemaining = dt; + var it = 0; + + var piTime = currentTime; + do { + if (timeRemaining <= 0) + break; + + var timeStep = 0.00800000037997961; + if (timeRemaining < 0.00800000037997961) + timeStep = timeRemaining; + + this.findContacts(collisionWorld, timeStep); + var cmf = this.computeMoveForces(m); + var isCentered:Bool = cmf.result; + var aControl = cmf.aControl; + var desiredOmega = cmf.desiredOmega; + this.velocityCancel(isCentered, false); + var A = this.getExternalForces(m, timeStep); + var retf = this.applyContactForces(timeStep, m, isCentered, aControl, desiredOmega, A); + A = retf[0]; + var a = retf[1]; + this.velocity = this.velocity.add(A.multiply(timeStep)); + this.omega = this.omega.add(a.multiply(timeStep)); + this.velocityCancel(isCentered, true); + this._totalTime += timeStep; + if (contacts.length != 0) { + this._contactTime += timeStep; + } + + var intersectT = this.getIntersectionTime(timeStep, velocity, pathedInteriors, collisionWorld); + + if (intersectT < timeStep) { + var diff = timeStep - intersectT; + this.velocity = this.velocity.sub(A.multiply(diff)); + this.omega = this.omega.sub(a.multiply(diff)); + timeStep = intersectT; + } + + piTime += timeStep; + if (this.controllable) { + for (interior in pathedInteriors) { + interior.update(piTime, timeStep); + } + } + + var pos = this.getAbsPos().getPosition(); + + var newPos = pos.add(this.velocity.multiply(timeStep)); + this.setPosition(newPos.x, newPos.y, newPos.z); + var tform = this.collider.transform; + tform.setPosition(new Vector(newPos.x, newPos.y, newPos.z)); + this.collider.setTransform(tform); + this.collider.velocity = this.velocity; + + timeRemaining -= timeStep; + it++; + } while (it <= 10); this.queuedContacts = []; } @@ -700,33 +812,62 @@ class Marble extends Object { } } - var timeRemaining = dt; - var it = 0; - do { - if (timeRemaining <= 0) - break; + advancePhysics(currentTime, dt, move, collisionWorld, pathedInteriors); - var timeStep = 0.00800000037997961; - if (timeRemaining < 0.00800000037997961) - timeStep = timeRemaining; + // var timeRemaining = dt; + // var it = 0; - advancePhysics(move, timeStep, collisionWorld); - var newPos = this.getAbsPos().getPosition().add(this.velocity.multiply(timeStep)); - this.setPosition(newPos.x, newPos.y, newPos.z); - var tform = this.collider.transform; - tform.setPosition(new Vector(newPos.x, newPos.y, newPos.z)); - this.collider.setTransform(tform); - this.collider.velocity = this.velocity; + // var piTime = currentTime; - timeRemaining -= timeStep; - it++; - } while (it <= 10); + // var pos = this.getAbsPos().getPosition(); + // do { + // if (timeRemaining <= 0) + // break; - if (this.controllable) { - for (interior in pathedInteriors) { - interior.update(currentTime, dt); - } - } + // var timeStep = 0.00800000037997961; + // if (timeRemaining < 0.00800000037997961) + // timeStep = timeRemaining; + + // var externalForces = getExternalForces(move, timeStep); + + // advancePhysics(move, timeStep, collisionWorld); + + // var intersectT = this.getIntersectionTime(timeStep, velocity, pathedInteriors, collisionWorld); + + // if (intersectT < timeStep) { + // timeStep = intersectT; + // } + + // piTime += timeStep; + + // if (this.controllable) { + // for (interior in pathedInteriors) { + // // interior.rollBack(); + // interior.update(piTime, timeStep); + // } + // } + + // var pos = this.getAbsPos().getPosition(); + + // var newPos = pos.add(this.velocity.multiply(timeStep)); + // this.setPosition(newPos.x, newPos.y, newPos.z); + // var tform = this.collider.transform; + // tform.setPosition(new Vector(newPos.x, newPos.y, newPos.z)); + // this.collider.setTransform(tform); + // this.collider.velocity = this.velocity; + + // // if (intersectT != timeStep && intersectT != 10e8) { + // // // this.velocity = this.velocity.sub(externalForces.multiply(timeStep - intersectT)); + // // // this.omega + // // if (intersectT > timeStep) { + // // trace("Bruh"); + // // } + // // timeStep = intersectT; + // // } + + // timeRemaining -= timeStep; + // it++; + // } while (it <= 10); this.camera.target.load(this.getAbsPos().getPosition().toPoint()); } diff --git a/src/PathedInterior.hx b/src/PathedInterior.hx index bb2fecd2..50d1e01a 100644 --- a/src/PathedInterior.hx +++ b/src/PathedInterior.hx @@ -6,18 +6,29 @@ import src.Util; import src.PathedInteriorMarker; import src.InteriorGeometry; -class PathedInterior extends InteriorGeometry { - public var markerData:Array = []; - - var duration:Float; +typedef PIState = { var currentTime:Float; var targetTime:Float; var changeTime:Float; - var prevPosition:Vector; var currentPosition:Vector; - var velocity:Vector; +} + +class PathedInterior extends InteriorGeometry { + public var markerData:Array = []; + + public var duration:Float; + public var currentTime:Float; + public var targetTime:Float; + public var changeTime:Float; + + public var prevPosition:Vector; + public var currentPosition:Vector; + + public var velocity:Vector; + + var previousState:PIState; public function new() { super(); @@ -29,6 +40,15 @@ class PathedInterior extends InteriorGeometry { } public function update(currentTime:Float, dt:Float) { + this.previousState = { + currentTime: currentTime, + targetTime: targetTime, + changeTime: changeTime, + prevPosition: prevPosition, + currentPosition: currentPosition, + velocity: velocity + }; + var transform = this.getTransformAtTime(this.getInternalTime(currentTime)); this.updatePosition(); @@ -39,6 +59,16 @@ class PathedInterior extends InteriorGeometry { velocity = position.sub(this.prevPosition).multiply(1 / dt); } + public function rollBack() { + this.currentTime = this.previousState.currentTime; + this.targetTime = this.previousState.targetTime; + this.changeTime = this.previousState.changeTime; + this.prevPosition = this.previousState.prevPosition; + this.currentPosition = this.previousState.currentPosition; + this.velocity = this.previousState.velocity; + this.updatePosition(); + } + function computeDuration() { var total = 0.0; for (marker in markerData) { diff --git a/src/collision/Collision.hx b/src/collision/Collision.hx index 6bb0a969..a42e381d 100644 --- a/src/collision/Collision.hx +++ b/src/collision/Collision.hx @@ -216,7 +216,7 @@ class Collision { return res; } - private static function PointInTriangle(point:Vector, v0:Vector, v1:Vector, v2:Vector):Bool { + public static function PointInTriangle(point:Vector, v0:Vector, v1:Vector, v2:Vector):Bool { var u = v1.sub(v0); var v = v2.sub(v0); var w = point.sub(v0); @@ -242,7 +242,7 @@ class Collision { return (r + t) <= 1; } - private static function PointInTriangle2(point:Vector, a:Vector, b:Vector, c:Vector):Bool { + public static function PointInTriangle2(point:Vector, a:Vector, b:Vector, c:Vector):Bool { var a1 = a.sub(point); var b1 = b.sub(point); var c1 = c.sub(point); diff --git a/src/collision/CollisionEntity.hx b/src/collision/CollisionEntity.hx index 393c508c..9c4af61e 100644 --- a/src/collision/CollisionEntity.hx +++ b/src/collision/CollisionEntity.hx @@ -74,7 +74,7 @@ class CollisionEntity implements IOctreeObject { var surfaces = octree.radiusSearch(localpos, radius * 1.1); var tform = transform.clone(); - // tform.setPosition(tform.getPosition().add(velocity.multiply(dt))); + tform.setPosition(tform.getPosition().add(this.velocity.multiply(dt))); var contacts = []; @@ -104,8 +104,10 @@ class CollisionEntity implements IOctreeObject { cinfo.point = closest; // cinfo.collider = this; cinfo.velocity = this.velocity; - cinfo.penetration = radius - (position.sub(closest).dot(normal)); + cinfo.contactDistance = closest.distance(position); + // cinfo.penetration = radius - (position.sub(closest).dot(normal)); cinfo.restitution = 1; + cinfo.force = 0; cinfo.friction = 1; contacts.push(cinfo); } diff --git a/src/collision/CollisionInfo.hx b/src/collision/CollisionInfo.hx index beb9d29d..9dcafa20 100644 --- a/src/collision/CollisionInfo.hx +++ b/src/collision/CollisionInfo.hx @@ -8,8 +8,11 @@ class CollisionInfo { public var velocity:Vector; public var collider:CollisionEntity; public var friction:Float; + public var vAtCMag:Float; + public var normalForce:Float; public var restitution:Float; - public var penetration:Float; + public var contactDistance:Float; + public var force:Float; public function new() {} } diff --git a/src/collision/CollisionWorld.hx b/src/collision/CollisionWorld.hx index 7ebca7fb..0003d0b7 100644 --- a/src/collision/CollisionWorld.hx +++ b/src/collision/CollisionWorld.hx @@ -53,12 +53,12 @@ class CollisionWorld { for (obj in intersections) { var entity:CollisionEntity = cast obj; - contacts = contacts.concat(entity); + contacts.push(entity); } for (obj in dynamicEntities) { if (obj.boundingBox.collide(box)) - contacts = contacts.concat(obj); + contacts.push(obj); } return contacts; diff --git a/src/collision/SphereCollisionEntity.hx b/src/collision/SphereCollisionEntity.hx index d8535948..0f836062 100644 --- a/src/collision/SphereCollisionEntity.hx +++ b/src/collision/SphereCollisionEntity.hx @@ -45,7 +45,9 @@ class SphereCollisionEntity extends CollisionEntity { contact.velocity = this.velocity; contact.point = position.add(thispos).multiply(0.5); contact.normal = contact.point.sub(thispos).normalized(); - contact.penetration = radius - (position.sub(contact.point).dot(contact.normal)); + contact.force = 0; + contact.contactDistance = contact.point.distance(position); + // contact.penetration = radius - (position.sub(contact.point).dot(contact.normal)); contacts.push(contact); var othercontact = new CollisionInfo(); @@ -55,7 +57,9 @@ class SphereCollisionEntity extends CollisionEntity { othercontact.velocity = this.velocity; othercontact.point = thispos.add(position).multiply(0.5); othercontact.normal = contact.point.sub(position).normalized(); - othercontact.penetration = this.radius - (thispos.sub(othercontact.point).dot(othercontact.normal)); + othercontact.contactDistance = contact.point.distance(position); + othercontact.force = 0; + // othercontact.penetration = this.radius - (thispos.sub(othercontact.point).dot(othercontact.normal)); this.marble.queueCollision(othercontact); } return contacts;