/*
* To change this license header, choose License Headers in Project Properties.
* To change this template file, choose Tools | Templates
* and open the template in the editor.
*/
package aspect.physics;
import aspect.entity.Entity;
import aspect.entity.behavior.Behavior;
import aspect.util.Matrix3x3;
import aspect.util.Matrix4x4;
import aspect.util.Vector3;
import java.util.Arrays;
/**
*
* @author MillerV
*/
public class Collider extends Behavior {
private final Vector3[] vertices;
private final Vector3[] normals;
private final Vector3[] edges;
private final float radius;
public final int numVertices;
public final int numNormals;
public final int numEdges;
public Collider(Vector3[] vertices, Vector3[] normals, Vector3[] edges) {
this.vertices = vertices;
this.normals = normals;
this.edges = edges;
this.numVertices = vertices.length;
this.numNormals = normals.length;
this.numEdges = edges.length;
float r = 0.0f;
for (Vector3 vertex : vertices) {
r = Math.max(r, vertex.mag());
}
this.radius = r;
}
public Vector3[] getVertices() {
Vector3[] tverts = new Vector3[numVertices];
Matrix4x4 m = ent.transform.getMdlMatrix();
for (int i = 0; i < numVertices; i++) {
tverts[i] = m.transformPoint(vertices[i]);
}
return tverts;
}
public Vector3[] getNormals() {
Vector3[] tnorms = new Vector3[numNormals];
getNormals(tnorms, 0);
return tnorms;
}
private int getNormals(Vector3[] v, int start) {
Matrix4x4 m = ent.transform.getMdlMatrix().invert().transpose();
for (int i = 0; i < numNormals; i++) {
v[start + i] = m.transformVector(normals[i]);
}
return numNormals;
}
public Vector3[] getEdges() {
Vector3[] tedges = new Vector3[numEdges];
getEdges(tedges, 0);
return tedges;
}
private int getEdges(Vector3[] v, int start) {
Matrix4x4 m = ent.transform.getMdlMatrix();
for (int i = 0; i < numEdges; i++) {
v[start + i] = m.transformVector(edges[i]);
}
return numEdges;
}
public boolean colliding(Collider c, Vector3 contact, Vector3 displacement, Vector3 normal) {
float maxRadius = radius + c.radius;
if (Vector3.distance2(ent.transform.position, c.ent.transform.position) > maxRadius * maxRadius) {
return false;
}
Vector3[] verts = getVertices();
Vector3[] cverts = c.getVertices();
int pos = 0;
Vector3[] axes = new Vector3[numNormals + c.numNormals + numEdges * c.numEdges];
pos += getNormals(axes, pos);
pos += c.getNormals(axes, pos);
Vector3[] edges = getEdges();
Vector3[] cedges = c.getEdges();
for (int i = 0; i < numEdges; i++) {
for (int j = 0; j < c.numEdges; j++) {
Vector3 cross = Vector3.cross(edges[i], cedges[j]);
if (cross.mag2() != 0.0f) {
axes[pos++] = cross.normalize();
} else {
axes[pos++] = null;
}
}
}
float dist = Float.POSITIVE_INFINITY;
int i = 0;
for (Vector3 n : axes) {
if (n == null) {
continue;
}
float min1 = Float.POSITIVE_INFINITY;
Vector3 min1v = null;
float max1 = Float.NEGATIVE_INFINITY;
Vector3 max1v = null;
float min2 = Float.POSITIVE_INFINITY;
Vector3 min2v = null;
float max2 = Float.NEGATIVE_INFINITY;
Vector3 max2v = null;
for (Vector3 v : verts) {
float proj = Vector3.dot(v, n);
if (proj < min1) {
min1 = proj;
min1v = v;
}
if (proj > max1) {
max1 = proj;
max1v = v;
}
}
for (Vector3 v : cverts) {
float proj = Vector3.dot(v, n);
if (proj < min2) {
min2 = proj;
min2v = v;
}
if (proj > max2) {
max2 = proj;
max2v = v;
}
}
if (min1 <= min2 && min2 <= max1 || min2 <= min1 && min1 <= max2) {
if (max1 - min2 < dist) {
dist = max1 - min2;
displacement.set(n.times(-dist));
normal.set(n.negate());
if (i < numNormals) {
contact.set(min2v);
} else if (i < numNormals + c.numNormals) {
contact.set(max1v.plus(displacement));
} else {
int e = i - (numNormals + c.numNormals);
Vector3 v1 = Vector3.cross(edges[e / c.numEdges], n);
Vector3 v2 = Vector3.cross(cedges[e % c.numEdges], n);
float off1 = Vector3.dot(v1, max1v);
float off2 = Vector3.dot(v2, min2v);
float off3 = Vector3.dot(n, min2v);
Vector3 v4 = new Vector3(off1, off2, off3);
Matrix3x3 m = new Matrix3x3(v1, v2, n);
contact.set(m.invert().transform(v4));
}
}
if (max2 - min1 < dist) {
dist = max2 - min1;
displacement.set(n.times(dist));
normal.set(n);
if (i < numNormals) {
contact.set(max2v);
} else if (i < numNormals + c.numNormals) {
contact.set(min1v.plus(displacement));
} else {
int e = i - (numNormals + c.numNormals);
Vector3 v1 = Vector3.cross(edges[e / c.numEdges], n);
Vector3 v2 = Vector3.cross(cedges[e % c.numEdges], n);
float off1 = Vector3.dot(v1, min1v);
float off2 = Vector3.dot(v2, max2v);
float off3 = Vector3.dot(n, max2v);
Vector3 v4 = new Vector3(off1, off2, off3);
Matrix3x3 m = new Matrix3x3(v1, v2, n);
contact.set(m.invert().transform(v4));
}
}
} else {
return false;
}
i++;
}
return true;
}
public static Collider rect(float w, float h) {
Vector3[] vertices = new Vector3[4];
Vector3[] normals = new Vector3[1];
Vector3[] edges = new Vector3[2];
w /= 2;
h /= 2;
vertices[0] = new Vector3(w, h);
vertices[1] = new Vector3(w, -h);
vertices[2] = new Vector3(-w, -h);
vertices[3] = new Vector3(-w, h);
normals[0] = Vector3.zAxis();
edges[0] = Vector3.yAxis();
edges[1] = Vector3.xAxis();
return new Collider(vertices, normals, edges);
}
public static Collider box(float w, float h, float d) {
Vector3[] vertices = new Vector3[8];
Vector3[] normals = new Vector3[3];
Vector3[] edges = normals;
w /= 2;
h /= 2;
d /= 2;
vertices[0] = new Vector3(w, h, d);
vertices[1] = new Vector3(w, h, -d);
vertices[2] = new Vector3(w, -h, -d);
vertices[3] = new Vector3(w, -h, d);
vertices[4] = new Vector3(-w, h, d);
vertices[5] = new Vector3(-w, h, -d);
vertices[6] = new Vector3(-w, -h, -d);
vertices[7] = new Vector3(-w, -h, d);
normals[0] = Vector3.xAxis();
normals[1] = Vector3.yAxis();
normals[2] = Vector3.zAxis();
return new Collider(vertices, normals, edges);
}
public Collider point(Vector3 point) {
return new Collider(new Vector3[]{point}, new Vector3[0], new Vector3[0]);
}
}