thirdpartymirrors/MBHaxe
1
0
Fork 0
mirror of https://github.com/RandomityGuy/MBHaxe.git synced 2026-04-26 12:41:40 +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; package src;
import h3d.Matrix;
import collision.CollisionEntity; import collision.CollisionEntity;
import headbutt.threed.Headbutt; import headbutt.threed.Headbutt;
import headbutt.threed.shapes.Sphere; import headbutt.threed.shapes.Sphere;
@ -17,4 +18,9 @@ class InteriorGeometry extends Object {
public function new() { public function new() {
super(); 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 velocity:Vector;
var omega:Vector; var omega:Vector;
var gravityDir:Vector = new Vector(0, 0, -1);
var _radius = 0.2; var _radius = 0.2;
var _maxRollVelocity = 15; var _maxRollVelocity = 15;
@ -66,34 +67,30 @@ class Marble extends Object {
} }
function getMarbleAxis() { function getMarbleAxis() {
var gravitydir = new Vector(0, 0, -1);
var cammat = Matrix.I(); var cammat = Matrix.I();
var xrot = new Matrix(); var xrot = new Matrix();
xrot.initRotationX(this.camera.phi); xrot.initRotationX(this.camera.phi);
var zrot = new Matrix(); var zrot = new Matrix();
zrot.initRotationZ(this.camera.theta); zrot.initRotationZ(this.camera.theta);
cammat.multiply(xrot, zrot); 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 motiondir = new Vector(cammat._21, cammat._22, cammat._23);
var sidedir = motiondir.clone().cross(updir); var sidedir = motiondir.cross(updir);
sidedir.normalize(); sidedir.normalize();
motiondir = updir.clone().cross(sidedir); motiondir = updir.cross(sidedir);
return [sidedir, motiondir, updir]; return [sidedir, motiondir, updir];
} }
function getExternalForces(m:Move, dt:Float, contacts:Array<CollisionInfo>) { function getExternalForces(m:Move, dt:Float, contacts:Array<CollisionInfo>) {
var gWorkGravityDir = new Vector(0, 0, -1); var gWorkGravityDir = gravityDir;
var A = gWorkGravityDir.clone().multiply(this._gravity); var A = gWorkGravityDir.multiply(this._gravity);
if (contacts.length == 0) { if (contacts.length == 0) {
var axes = this.getMarbleAxis(); var axes = this.getMarbleAxis();
var sideDir = axes[0]; var sideDir = axes[0];
var motionDir = axes[1]; var motionDir = axes[1];
var upDir = axes[2]; var upDir = axes[2];
A = A.add(sideDir.clone() A = A.add(sideDir.multiply(m.d.x).add(motionDir.multiply(m.d.y)).multiply(this._airAccel));
.multiply(m.d.x)
.add(motionDir.clone().multiply(m.d.y))
.multiply(this._airAccel));
} }
return A; return A;
} }
@ -101,9 +98,9 @@ class Marble extends Object {
function computeMoveForces(m:Move) { function computeMoveForces(m:Move) {
var aControl = new Vector(); var aControl = new Vector();
var desiredOmega = new Vector(); var desiredOmega = new Vector();
var currentGravityDir = new Vector(0, 0, -1); var currentGravityDir = gravityDir;
var R = currentGravityDir.clone().multiply(-this._radius); var R = currentGravityDir.multiply(-this._radius);
var rollVelocity = this.omega.clone().cross(R); var rollVelocity = this.omega.cross(R);
var axes = this.getMarbleAxis(); var axes = this.getMarbleAxis();
var sideDir = axes[0]; var sideDir = axes[0];
var motionDir = axes[1]; var motionDir = axes[1];
@ -112,7 +109,7 @@ class Marble extends Object {
var currentXVelocity = rollVelocity.dot(sideDir); var currentXVelocity = rollVelocity.dot(sideDir);
var mv = m.d; var mv = m.d;
mv = mv.multiply(1.53846157); mv = mv.multiply(1.538461565971375);
var mvlen = mv.length(); var mvlen = mv.length();
if (mvlen > 1) { if (mvlen > 1) {
mv = mv.multiply(1 / mvlen); mv = mv.multiply(1 / mvlen);
@ -131,8 +128,8 @@ class Marble extends Object {
desiredXVelocity = currentXVelocity; desiredXVelocity = currentXVelocity;
} }
var rsq = R.lengthSq(); var rsq = R.lengthSq();
desiredOmega = R.clone().cross(motionDir.clone().multiply(desiredYVelocity).add(sideDir.clone().multiply(desiredXVelocity))).multiply(1 / rsq); desiredOmega = R.cross(motionDir.multiply(desiredYVelocity).add(sideDir.multiply(desiredXVelocity))).multiply(1 / rsq);
aControl = desiredOmega.clone().sub(this.omega); aControl = desiredOmega.sub(this.omega);
var aScalar = aControl.length(); var aScalar = aControl.length();
if (aScalar > this._angularAcceleration) { if (aScalar > this._angularAcceleration) {
aControl = aControl.multiply(this._angularAcceleration / aScalar); aControl = aControl.multiply(this._angularAcceleration / aScalar);
@ -151,18 +148,18 @@ class Marble extends Object {
done = true; done = true;
itersIn++; itersIn++;
for (i in 0...contacts.length) { 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); var surfaceDot = contacts[i].normal.dot(sVel);
if ((!looped && surfaceDot < 0) || surfaceDot < -SurfaceDotThreshold) { if ((!looped && surfaceDot < 0) || surfaceDot < -SurfaceDotThreshold) {
var velLen = this.velocity.length(); 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); this.ReportBounce(contacts[i].point, contacts[i].normal, -surfaceDot);
if (noBounce) { if (noBounce) {
this.velocity = this.velocity.sub(surfaceVel); this.velocity = this.velocity.sub(surfaceVel);
} else if (contacts[i].collider != null) { } else if (contacts[i].collider != null) {
var info = contacts[i]; var info = contacts[i];
var bounce2 = 0.5; 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); normV = normV.multiply(1 + bounce2);
this.velocity = this.velocity.sub(normV); this.velocity = this.velocity.sub(normV);
} else { } else {
@ -181,10 +178,10 @@ class Marble extends Object {
} else { } else {
var restitution = this._bounceRestitution; var restitution = this._bounceRestitution;
restitution *= contacts[i].restitution; restitution *= contacts[i].restitution;
var velocityAdd = surfaceVel.clone().multiply(-(1 + restitution)); var velocityAdd = surfaceVel.multiply(-(1 + restitution));
var vAtC = sVel.clone().add(this.omega.clone().cross(contacts[i].normal.clone().multiply(this._radius).multiply(-1))); var vAtC = sVel.add(this.omega.cross(contacts[i].normal.multiply(-this._radius)));
var normalVel = -contacts[i].normal.dot(sVel); 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(); var vAtCMag = vAtC.length();
if (vAtCMag != 0) { if (vAtCMag != 0) {
var friction = this._bounceKineticFriction * contacts[i].friction; var friction = this._bounceKineticFriction * contacts[i].friction;
@ -192,13 +189,10 @@ class Marble extends Object {
if (angVMagnitude > vAtCMag / this._radius) { if (angVMagnitude > vAtCMag / this._radius) {
angVMagnitude = vAtCMag / this._radius; angVMagnitude = vAtCMag / this._radius;
} }
var vAtCDir = vAtC.clone().multiply(1 / vAtCMag); var vAtCDir = vAtC.multiply(1 / vAtCMag);
var deltaOmega = contacts[i].normal.clone() var deltaOmega = contacts[i].normal.cross(vAtCDir).multiply(angVMagnitude);
.multiply(-1)
.cross(vAtCDir.clone().multiply(-1))
.multiply(angVMagnitude);
this.omega = this.omega.add(deltaOmega); 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); this.velocity = this.velocity.add(velocityAdd);
} }
@ -211,37 +205,38 @@ class Marble extends Object {
done = true; done = true;
} }
} while (!done); } while (!done);
// if (velocity.LengthSquared() < 625f) if (this.velocity.lengthSq() < 625) {
// { var gotOne = false;
var gotOne = false; var dir = new Vector(0, 0, 0);
var dir = new Vector(0, 0, 0); for (j in 0...contacts.length) {
for (j in 0...contacts.length) { var dir2 = dir.add(contacts[j].normal);
var dir2 = dir.clone().add(contacts[j].normal); if (dir2.lengthSq() < 0.01) {
if (dir2.lengthSq() < 0.01) { dir2 = dir2.add(contacts[j].normal);
dir2 = dir2.add(contacts[j].normal); }
dir = dir2;
gotOne = true;
} }
dir = dir2; if (gotOne) {
gotOne = true; dir.normalize();
} var soFar = 0.0;
if (gotOne) { for (k in 0...contacts.length) {
dir.normalize(); if (contacts[k].penetration < this._radius) {
var soFar = 0.0; var timeToSeparate = 0.1;
for (k in 0...contacts.length) { var dist = contacts[k].penetration;
if (contacts[k].penetration < this._radius) { var outVel = this.velocity.add(dir.multiply(soFar)).dot(contacts[k].normal);
var timeToSeparate = 0.1;
var dist = contacts[k].penetration;
var outVel = this.velocity.clone().add(dir.clone().multiply(soFar)).dot(contacts[k].normal);
if (timeToSeparate * outVel < dist) { if (timeToSeparate * outVel < dist) {
soFar += (dist - outVel * timeToSeparate) / timeToSeparate / contacts[k].normal.dot(dir); 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>) { 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 priority:Int;
public var position:Int; public var position:Int;
var transform:Matrix; public var transform:Matrix;
public function new() { public function new() {
this.octree = new Octree(); this.octree = new Octree();
@ -33,6 +33,11 @@ class CollisionEntity implements IOctreeObject {
this.surfaces.push(surface); this.surfaces.push(surface);
} }
public function setTransform(transform:Matrix) {
this.transform = transform;
generateBoundingBox();
}
public function generateBoundingBox() { public function generateBoundingBox() {
var boundingBox = new Bounds(); var boundingBox = new Bounds();
for (surface in this.surfaces) { for (surface in this.surfaces) {
@ -61,120 +66,29 @@ class CollisionEntity implements IOctreeObject {
invMatrix.invert(); invMatrix.invert();
var localpos = position.clone(); var localpos = position.clone();
localpos.transform(invMatrix); localpos.transform(invMatrix);
var surfaces = octree.radiusSearch(localpos, radius * 1.1);
var bigRad = 2 * velocity.length() + radius * 1.1;
var surfaces = octree.radiusSearch(position, radius * 1.1);
var contacts = []; 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) { for (obj in surfaces) {
var surface:CollisionSurface = cast obj; var surface:CollisionSurface = cast obj;
var i = 0; var i = 0;
while (i < surface.indices.length) { while (i < surface.indices.length) {
var v0 = surface.points[surface.indices[i]]; var v0 = surface.points[surface.indices[i]].transformed(transform);
var v = surface.points[surface.indices[i + 1]]; var v = surface.points[surface.indices[i + 1]].transformed(transform);
var v2 = surface.points[surface.indices[i + 2]]; var v2 = surface.points[surface.indices[i + 2]].transformed(transform);
// var packet = new CollisionPacket(position, velocity, new Vector(radius, radius, radius)); var surfacenormal = surface.normals[surface.indices[i]].transformed(transform);
// packet.e_base_point = packet.e_position.clone();
// packet.e_norm_velocity = packet.e_velocity.clone().normalized();
// packet.nearest_distance = 1e20;
// var plane = PlaneF.PointNormal(toDifPoint(v), toDifPoint(surface.normals[surface.indices[i]])); var res = Collision.IntersectTriangleSphere(v0, v, v2, surfacenormal, position, radius);
// 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 closest = res.point; var closest = res.point;
// Collision.ClosestPtPointTriangle(position, radius, v0, v, v2, surface.normals[surface.indices[i]]); // Collision.ClosestPtPointTriangle(position, radius, v0, v, v2, surface.normals[surface.indices[i]]);
if (res.result) { if (res.result) {
if (position.sub(closest).lengthSq() < radius * radius) { if (position.sub(closest).lengthSq() < radius * radius) {
var normal = res.normal; var normal = res.normal;
if (position.sub(closest).dot(surface.normals[surface.indices[i]]) > 0) { if (position.sub(closest).dot(surfacenormal) > 0) {
normal.normalize(); normal.normalize();
var cinfo = new CollisionInfo(); 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; i += 3;
} }
} }