/*
* Copyright (C) 2014 MillerV
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
package aspect.physics;
import aspect.entity.Entity;
import aspect.physics.dynamics.Force;
import aspect.render.Material;
import aspect.resources.Resources;
import aspect.util.Matrix3x3;
import aspect.util.Vector3;
import aspect.world.World;
import java.util.LinkedList;
/**
* This is currently just an extension of Motion that includes force and mass.
* It is not a full implementation of rigid body physics.
*
* @author MillerV
*/
public class RigidBody extends Motion {
public float mass = 1f; // kg
public Matrix3x3 inertiaTensor; // kg * m ^ 2
public float coefficientOfFriction = 0.25f;
public float coefficientOfRestitution = 0f;
public LinkedList<Force> forces = new LinkedList<>();
public World world;
public RigidBody() {
this(World.main);
}
public RigidBody(World world) {
super();
this.world = world;
this.inertiaTensor = Matrix3x3.identity();
}
public float momentOfInertia(Vector3 axis) {
Vector3 axisLocal = ent.transform.getCamMatrix().transformVector(axis);
return Vector3.dot(axisLocal, inertiaTensor.transform(axisLocal));
}
public void setMomentsOfInertia(Vector3 moi) {
inertiaTensor = Matrix3x3.zero();
inertiaTensor.m00 = moi.x;
inertiaTensor.m11 = moi.y;
inertiaTensor.m22 = moi.z;
}
public void setUniformMOI(float moi) {
inertiaTensor = Matrix3x3.zero();
inertiaTensor.m00 = moi;
inertiaTensor.m11 = moi;
inertiaTensor.m22 = moi;
}
public void impel(Vector3 impulse) {
velocity = velocity.plus(Vector3.divide(impulse, mass));
}
public void impel(Vector3 impulse, Vector3 point) {
Vector3 angularImpulse = Vector3.cross(point, impulse);
if (ent == null || angularImpulse.mag2() == 0.0f) {
return;
}
impel(impulse);
impelAngular(angularImpulse);
}
public void impelAngular(Vector3 impulse) {
angularVelocity = angularVelocity.plus(Vector3.divide(impulse, momentOfInertia(impulse.normalize())));
}
public void addForce(Force force) {
forces.add(force);
}
public Vector3 netForce() {
Vector3 sigmaForce = Vector3.zero();
for (Force force : forces) {
sigmaForce = sigmaForce.plus(force.getForce(ent));
}
if (world != null) {
for (Force force : world.forces()) {
sigmaForce = sigmaForce.plus(force.getForce(ent));
}
}
return sigmaForce;
}
public void checkCollision(Collider c) {
Collider collider = ent.collider();
if (collider == null || c == null) {
return;
}
Vector3 contact = new Vector3(0.0f);
Vector3 displacement = new Vector3(0.0f);
Vector3 normal = new Vector3(0.0f);
boolean b = collider.colliding(c, contact, displacement, normal);
if (b) {
//ent.transform.position = ent.transform.position.plus(displacement);
boolean both = true;
RigidBody rb2 = c.ent.rigidBody();
float moiA, moiB;
if (rb2 == null) {
rb2 = new RigidBody();
rb2.mass = Float.POSITIVE_INFINITY;
both = false;
}
Vector3 pos1 = contact.minus(ent.transform.position);
Vector3 pos2 = contact.minus(c.ent.transform.position);
Vector3 va = velocity.plus(Vector3.cross(angularVelocity, pos1));
Vector3 vb = rb2.velocity.plus(Vector3.cross(rb2.angularVelocity, pos2));
if (rb2.ent == null) {
ent.transform.position = ent.transform.position.plus(displacement);
} else {
Vector3 vt = va.plus(vb);
float magt = vt.mag();
if (magt == 0.0f) {
ent.transform.position = ent.transform.position.plus(displacement);
} else {
float f1 = va.mag() / magt;
float f2 = 1.0f - f1;
ent.transform.position = ent.transform.position.plus(displacement.times(f1));
rb2.ent.transform.position = rb2.ent.transform.position.minus(displacement.times(f2));
}
}
Vector3 vr = va.minus(vb);
// Normal impulse
float num = -Vector3.dot(vr.times(1 + coefficientOfRestitution), normal);
float f = 1.0f / mass + 1.0f / rb2.mass;
Vector3 n2 = normal.times(f);
float denom1 = Vector3.dot(normal, n2);
moiA = momentOfInertia(Vector3.cross(normal, pos1).normalize());
if (both) {
moiB = rb2.momentOfInertia(Vector3.cross(normal, pos2).normalize());
} else {
moiB = Float.POSITIVE_INFINITY;
}
Vector3 cross1 = Vector3.cross(Vector3.divide(Vector3.cross(pos1, normal), moiA), pos1);
Vector3 cross2 = Vector3.cross(Vector3.divide(Vector3.cross(pos2, normal), moiB), pos2);
Vector3 cross = cross1.plus(cross2);
float denom2 = Vector3.dot(normal, cross);
float denom = denom1 + denom2;
float impulse = num / denom;
Vector3 impulseN = normal.times(impulse);
impel(impulseN, pos1);
rb2.impel(impulseN.negate(), pos2);
// Frictional impulse
Vector3 tangent = Vector3.cross(Vector3.cross(vr, normal), normal).normalize();
if (tangent.mag2() == 0.0f) {
return;
}
num = -Vector3.dot(vr.times(coefficientOfFriction), tangent);
Vector3 t2 = tangent.times(f);
denom1 = Vector3.dot(tangent, t2);
moiA = momentOfInertia(Vector3.cross(tangent, pos1).normalize());
if (both) {
moiB = rb2.momentOfInertia(Vector3.cross(tangent, pos2).normalize());
} else {
moiB = Float.POSITIVE_INFINITY;
}
cross1 = Vector3.cross(Vector3.divide(Vector3.cross(pos1, tangent), moiA), pos1);
cross2 = Vector3.cross(Vector3.divide(Vector3.cross(pos2, tangent), moiB), pos2);
cross = cross1.plus(cross2);
denom2 = Vector3.dot(tangent, cross);
denom = denom1 + denom2;
float friction = num / denom;
Vector3 frictionT = tangent.times(friction);
impel(frictionT, pos1);
rb2.impel(frictionT.negate(), pos2);
//Vector3 friction = Vector3.cross(Vector3.cross(impulse, normal), normal).times(coefficientOfFriction);
//impel(friction, pos1);
}
}
@Override
public void update() {
acceleration = Vector3.divide(netForce(), mass);
super.update();
// Collisions
boolean checking = false;
for (Entity entity : world) {
if (entity == ent) {
checking = true;
continue;
}
if (checking || entity.rigidBody() == null) {
checkCollision(entity.collider());
}
}
}
public static Entity box(Material material, float width, float height, float depth, float density) {
Entity entity = new Entity(Resources.box(material, width, height, depth));
entity.addBehavior(Collider.box(width, height, depth));
entity.addBehavior(new RigidBody());
entity.rigidBody().mass = width * height * depth * density;
float w2 = width * width;
float h2 = height * height;
float d2 = depth * depth;
float f = entity.rigidBody().mass / 12.0f;
entity.rigidBody().setMomentsOfInertia(new Vector3(f * (h2 + d2), f * (w2 + d2), f * (w2 + h2)));
return entity;
}
}