thirdpartymirrors/MBHaxe
1
0
Fork 0
mirror of https://github.com/RandomityGuy/MBHaxe.git synced 2026-01-15 03:32:08 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions

physics

Browse source
This commit is contained in:
RandomityGuy 2021-05-28 17:52:57 +05:30
parent 13e8bc70ff
commit d67bc00810
3 changed files with 67 additions and 175 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

6
src/InteriorGeometry.hx
View file

@ -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);
}
}

101
src/Marble.hx
View file

@ -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<CollisionInfo>) {
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<CollisionInfo>) {

135
src/collision/CollisionEntity.hx
View file

@ -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;
}
}