From d67bc0081054dfa9c3264eed170803283f1c2ffa Mon Sep 17 00:00:00 2001 From: RandomityGuy <31925790+RandomityGuy@users.noreply.github.com> Date: Fri, 28 May 2021 17:52:57 +0530 Subject: [PATCH] physics --- src/InteriorGeometry.hx | 6 ++ src/Marble.hx | 101 +++++++++++------------ src/collision/CollisionEntity.hx | 135 +++---------------------------- 3 files changed, 67 insertions(+), 175 deletions(-) diff --git a/src/InteriorGeometry.hx b/src/InteriorGeometry.hx index 583fbabc..91d20a8e 100644 --- a/src/InteriorGeometry.hx +++ b/src/InteriorGeometry.hx @@ -1,5 +1,6 @@ package src; +import h3d.Matrix; import collision.CollisionEntity; import headbutt.threed.Headbutt; import headbutt.threed.shapes.Sphere; @@ -17,4 +18,9 @@ class InteriorGeometry extends Object { public function new() { super(); } + + public override function setTransform(transform:Matrix) { + super.setTransform(transform); + collider.setTransform(transform); + } } diff --git a/src/Marble.hx b/src/Marble.hx index eadd78ac..9724ddd1 100644 --- a/src/Marble.hx +++ b/src/Marble.hx @@ -26,6 +26,7 @@ class Marble extends Object { var velocity:Vector; var omega:Vector; + var gravityDir:Vector = new Vector(0, 0, -1); var _radius = 0.2; var _maxRollVelocity = 15; @@ -66,34 +67,30 @@ class Marble extends Object { } function getMarbleAxis() { - var gravitydir = new Vector(0, 0, -1); var cammat = Matrix.I(); var xrot = new Matrix(); xrot.initRotationX(this.camera.phi); var zrot = new Matrix(); zrot.initRotationZ(this.camera.theta); cammat.multiply(xrot, zrot); - var updir = new Vector(0, 0, 1); + var updir = gravityDir.multiply(-1); var motiondir = new Vector(cammat._21, cammat._22, cammat._23); - var sidedir = motiondir.clone().cross(updir); + var sidedir = motiondir.cross(updir); sidedir.normalize(); - motiondir = updir.clone().cross(sidedir); + motiondir = updir.cross(sidedir); return [sidedir, motiondir, updir]; } function getExternalForces(m:Move, dt:Float, contacts:Array) { - var gWorkGravityDir = new Vector(0, 0, -1); - var A = gWorkGravityDir.clone().multiply(this._gravity); + var gWorkGravityDir = gravityDir; + var A = gWorkGravityDir.multiply(this._gravity); if (contacts.length == 0) { var axes = this.getMarbleAxis(); var sideDir = axes[0]; var motionDir = axes[1]; var upDir = axes[2]; - A = A.add(sideDir.clone() - .multiply(m.d.x) - .add(motionDir.clone().multiply(m.d.y)) - .multiply(this._airAccel)); + A = A.add(sideDir.multiply(m.d.x).add(motionDir.multiply(m.d.y)).multiply(this._airAccel)); } return A; } @@ -101,9 +98,9 @@ class Marble extends Object { function computeMoveForces(m:Move) { var aControl = new Vector(); var desiredOmega = new Vector(); - var currentGravityDir = new Vector(0, 0, -1); - var R = currentGravityDir.clone().multiply(-this._radius); - var rollVelocity = this.omega.clone().cross(R); + var currentGravityDir = gravityDir; + var R = currentGravityDir.multiply(-this._radius); + var rollVelocity = this.omega.cross(R); var axes = this.getMarbleAxis(); var sideDir = axes[0]; var motionDir = axes[1]; @@ -112,7 +109,7 @@ class Marble extends Object { var currentXVelocity = rollVelocity.dot(sideDir); var mv = m.d; - mv = mv.multiply(1.53846157); + mv = mv.multiply(1.538461565971375); var mvlen = mv.length(); if (mvlen > 1) { mv = mv.multiply(1 / mvlen); @@ -131,8 +128,8 @@ class Marble extends Object { desiredXVelocity = currentXVelocity; } var rsq = R.lengthSq(); - desiredOmega = R.clone().cross(motionDir.clone().multiply(desiredYVelocity).add(sideDir.clone().multiply(desiredXVelocity))).multiply(1 / rsq); - aControl = desiredOmega.clone().sub(this.omega); + desiredOmega = R.cross(motionDir.multiply(desiredYVelocity).add(sideDir.multiply(desiredXVelocity))).multiply(1 / rsq); + aControl = desiredOmega.sub(this.omega); var aScalar = aControl.length(); if (aScalar > this._angularAcceleration) { aControl = aControl.multiply(this._angularAcceleration / aScalar); @@ -151,18 +148,18 @@ class Marble extends Object { done = true; itersIn++; for (i in 0...contacts.length) { - var sVel = this.velocity.clone().sub(contacts[i].velocity); + var sVel = this.velocity.sub(contacts[i].velocity); var surfaceDot = contacts[i].normal.dot(sVel); if ((!looped && surfaceDot < 0) || surfaceDot < -SurfaceDotThreshold) { var velLen = this.velocity.length(); - var surfaceVel = contacts[i].normal.clone().multiply(surfaceDot); + var surfaceVel = contacts[i].normal.multiply(surfaceDot); this.ReportBounce(contacts[i].point, contacts[i].normal, -surfaceDot); if (noBounce) { this.velocity = this.velocity.sub(surfaceVel); } else if (contacts[i].collider != null) { var info = contacts[i]; var bounce2 = 0.5; - var normV = info.normal.clone().multiply(this.velocity.dot(info.normal)); + var normV = info.normal.multiply(this.velocity.dot(info.normal)); normV = normV.multiply(1 + bounce2); this.velocity = this.velocity.sub(normV); } else { @@ -181,10 +178,10 @@ class Marble extends Object { } else { var restitution = this._bounceRestitution; restitution *= contacts[i].restitution; - var velocityAdd = surfaceVel.clone().multiply(-(1 + restitution)); - var vAtC = sVel.clone().add(this.omega.clone().cross(contacts[i].normal.clone().multiply(this._radius).multiply(-1))); + var velocityAdd = surfaceVel.multiply(-(1 + restitution)); + var vAtC = sVel.add(this.omega.cross(contacts[i].normal.multiply(-this._radius))); var normalVel = -contacts[i].normal.dot(sVel); - vAtC = vAtC.sub(contacts[i].normal.clone().multiply(contacts[i].normal.dot(sVel))); + vAtC = vAtC.sub(contacts[i].normal.multiply(contacts[i].normal.dot(sVel))); var vAtCMag = vAtC.length(); if (vAtCMag != 0) { var friction = this._bounceKineticFriction * contacts[i].friction; @@ -192,13 +189,10 @@ class Marble extends Object { if (angVMagnitude > vAtCMag / this._radius) { angVMagnitude = vAtCMag / this._radius; } - var vAtCDir = vAtC.clone().multiply(1 / vAtCMag); - var deltaOmega = contacts[i].normal.clone() - .multiply(-1) - .cross(vAtCDir.clone().multiply(-1)) - .multiply(angVMagnitude); + var vAtCDir = vAtC.multiply(1 / vAtCMag); + var deltaOmega = contacts[i].normal.cross(vAtCDir).multiply(angVMagnitude); this.omega = this.omega.add(deltaOmega); - this.velocity = this.velocity.sub(deltaOmega.clone().multiply(-1).cross(contacts[i].normal.clone().multiply(-this._radius))); + this.velocity = this.velocity.sub(deltaOmega.cross(contacts[i].normal.multiply(this._radius))); } this.velocity = this.velocity.add(velocityAdd); } @@ -211,37 +205,38 @@ class Marble extends Object { done = true; } } while (!done); - // if (velocity.LengthSquared() < 625f) - // { - var gotOne = false; - var dir = new Vector(0, 0, 0); - for (j in 0...contacts.length) { - var dir2 = dir.clone().add(contacts[j].normal); - if (dir2.lengthSq() < 0.01) { - dir2 = dir2.add(contacts[j].normal); + if (this.velocity.lengthSq() < 625) { + var gotOne = false; + var dir = new Vector(0, 0, 0); + for (j in 0...contacts.length) { + var dir2 = dir.add(contacts[j].normal); + if (dir2.lengthSq() < 0.01) { + dir2 = dir2.add(contacts[j].normal); + } + dir = dir2; + gotOne = true; } - dir = dir2; - gotOne = true; - } - if (gotOne) { - dir.normalize(); - var soFar = 0.0; - for (k in 0...contacts.length) { - if (contacts[k].penetration < this._radius) { - var timeToSeparate = 0.1; - var dist = contacts[k].penetration; - var outVel = this.velocity.clone().add(dir.clone().multiply(soFar)).dot(contacts[k].normal); + if (gotOne) { + dir.normalize(); + var soFar = 0.0; + for (k in 0...contacts.length) { + if (contacts[k].penetration < this._radius) { + var timeToSeparate = 0.1; + var dist = contacts[k].penetration; + var outVel = this.velocity.add(dir.multiply(soFar)).dot(contacts[k].normal); - if (timeToSeparate * outVel < dist) { - soFar += (dist - outVel * timeToSeparate) / timeToSeparate / contacts[k].normal.dot(dir); + if (timeToSeparate * outVel < dist) { + soFar += (dist - outVel * timeToSeparate) / timeToSeparate / contacts[k].normal.dot(dir); + } } } + if (soFar < -25) + soFar = -25; + if (soFar > 25) + soFar = 25; + this.velocity = this.velocity.add(dir.multiply(soFar)); } - // if (soFar < -25) soFar = -25; - // if (soFar > 25) soFar = 25; - this.velocity = this.velocity.add(dir.clone().multiply(soFar)); } - // } } function applyContactForces(dt:Float, m:Move, isCentered:Bool, aControl:Vector, desiredOmega:Vector, A:Vector, contacts:Array) { diff --git a/src/collision/CollisionEntity.hx b/src/collision/CollisionEntity.hx index 39ad2b96..104a75d0 100644 --- a/src/collision/CollisionEntity.hx +++ b/src/collision/CollisionEntity.hx @@ -20,7 +20,7 @@ class CollisionEntity implements IOctreeObject { public var priority:Int; public var position:Int; - var transform:Matrix; + public var transform:Matrix; public function new() { this.octree = new Octree(); @@ -33,6 +33,11 @@ class CollisionEntity implements IOctreeObject { this.surfaces.push(surface); } + public function setTransform(transform:Matrix) { + this.transform = transform; + generateBoundingBox(); + } + public function generateBoundingBox() { var boundingBox = new Bounds(); for (surface in this.surfaces) { @@ -61,120 +66,29 @@ class CollisionEntity implements IOctreeObject { invMatrix.invert(); var localpos = position.clone(); localpos.transform(invMatrix); - - var bigRad = 2 * velocity.length() + radius * 1.1; - - var surfaces = octree.radiusSearch(position, radius * 1.1); + var surfaces = octree.radiusSearch(localpos, radius * 1.1); var contacts = []; - function toDifPoint(pt:Vector) { - return new Point3F(pt.x, pt.y, pt.z); - } - - function fromDifPoint(pt:Point3F) { - return new Vector(pt.x, pt.y, pt.z); - } - for (obj in surfaces) { var surface:CollisionSurface = cast obj; var i = 0; while (i < surface.indices.length) { - var v0 = surface.points[surface.indices[i]]; - var v = surface.points[surface.indices[i + 1]]; - var v2 = surface.points[surface.indices[i + 2]]; + var v0 = surface.points[surface.indices[i]].transformed(transform); + var v = surface.points[surface.indices[i + 1]].transformed(transform); + var v2 = surface.points[surface.indices[i + 2]].transformed(transform); - // var packet = new CollisionPacket(position, velocity, new Vector(radius, radius, radius)); - // packet.e_base_point = packet.e_position.clone(); - // packet.e_norm_velocity = packet.e_velocity.clone().normalized(); - // packet.nearest_distance = 1e20; + var surfacenormal = surface.normals[surface.indices[i]].transformed(transform); - // var plane = PlaneF.PointNormal(toDifPoint(v), toDifPoint(surface.normals[surface.indices[i]])); - - // var retpacket = Collision.CheckTriangle(packet, v0.multiply(1 / radius), v.multiply(1 / radius), v2.multiply(1 / radius)); - - // if (retpacket.found_collision) { - // var cinfo = new CollisionInfo(); - // cinfo.restitution = 1; - // cinfo.friction = 1; - // cinfo.normal = surface.normals[surface.indices[i]]; - // cinfo.point = retpacket.intersect_point; - // cinfo.velocity = new Vector(); - // cinfo.collider = null; - // cinfo.penetration = radius - (position.sub(cinfo.point).dot(cinfo.normal)); - // contacts.push(cinfo); - // } - // var plane = PlaneF.ThreePoints(toDifPoint(v0), toDifPoint(v), toDifPoint(v2)); - - // var distance = plane.distance(toDifPoint(position)); - - // if (Math.abs(distance) <= radius + 0.001) { - // var lastVertex = surface.points[surface.indices[surface.indices.length - 1]]; - - // var contactVert = plane.project(toDifPoint(position)); - // var separation = Math.sqrt(radius * radius - distance * distance); - - // for (j in 0...surface.indices.length) { - // var vertex = surface.points[surface.indices[i]]; - // if (vertex != lastVertex) { - // var vertPlane = PlaneF.ThreePoints(toDifPoint(vertex).add(plane.getNormal()), toDifPoint(vertex), toDifPoint(lastVertex)); - // var vertDistance = vertPlane.distance(contactVert); - // if (vertDistance < 0.0) { - // if (vertDistance < -(separation + 0.0001)) - // return contacts; - // // return contacts; - - // if (PlaneF.ThreePoints(vertPlane.getNormal().add(toDifPoint(vertex)), toDifPoint(vertex), - // toDifPoint(vertex).add(plane.getNormal())) - // .distance(contactVert) >= 0.0) { - // if (PlaneF.ThreePoints(toDifPoint(lastVertex).sub(vertPlane.getNormal()), toDifPoint(lastVertex), - // toDifPoint(lastVertex).add(plane.getNormal())) - // .distance(contactVert) >= 0.0) { - // contactVert = vertPlane.project(contactVert); - // break; - // } - // contactVert = toDifPoint(lastVertex); - // } else { - // contactVert = toDifPoint(vertex); - // } - // } - // lastVertex = vertex; - // } - - // var cinfo = new CollisionInfo(); - // cinfo.restitution = 1; - // cinfo.friction = 1; - // cinfo.normal = surface.normals[i]; - // cinfo.point = fromDifPoint(contactVert); - // cinfo.velocity = new Vector(); - // cinfo.collider = null; - // cinfo.penetration = radius - (position.sub(cinfo.point).dot(cinfo.normal)); - // contacts.push(cinfo); - // } - // } - - // // var norm = Plane.fromPoints(v0.toPoint(), v.toPoint(), v2.toPoint()); - - // var cinfo = new CollisionInfo(); - // cinfo.restitution = 1; - // cinfo.friction = 1; - // cinfo.normal = surface.normals[i]; - // cinfo.point = fromDifPoint(contactVert); - // cinfo.velocity = new Vector(); - // cinfo.collider = null; - // cinfo.penetration = radius - (position.sub(cinfo.point).dot(cinfo.normal)); - // contacts.push(cinfo); - // } - - var res = Collision.IntersectTriangleSphere(v0, v, v2, surface.normals[surface.indices[i]], position, radius); + var res = Collision.IntersectTriangleSphere(v0, v, v2, surfacenormal, position, radius); var closest = res.point; // Collision.ClosestPtPointTriangle(position, radius, v0, v, v2, surface.normals[surface.indices[i]]); if (res.result) { if (position.sub(closest).lengthSq() < radius * radius) { var normal = res.normal; - if (position.sub(closest).dot(surface.normals[surface.indices[i]]) > 0) { + if (position.sub(closest).dot(surfacenormal) > 0) { normal.normalize(); var cinfo = new CollisionInfo(); @@ -190,29 +104,6 @@ class CollisionEntity implements IOctreeObject { } } - // var res = Collision.IntersectTriangleSphere(v0, v, v2, surface.normals[surface.indices[i]], position, radius); - // var closest = res.point; - // var closest = Collision.ClosestPtPointTriangle(position, radius, v0, v, v2, surface.normals[surface.indices[i]]); - // if (closest != null) { - // if (position.sub(closest).lengthSq() < radius * radius) { - // var normal = position.sub(closest); - - // if (position.sub(closest).dot(surface.normals[surface.indices[i]]) > 0) { - // normal.normalize(); - - // var cinfo = new CollisionInfo(); - // cinfo.normal = normal; - // cinfo.point = closest; - // // cinfo.collider = this; - // cinfo.velocity = new Vector(); - // cinfo.penetration = radius - (position.sub(closest).dot(normal)); - // cinfo.restitution = 1; - // cinfo.friction = 1; - // contacts.push(cinfo); - // } - // } - // } - i += 3; } }