package framework.model3D;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.HashSet;
import java.util.List;
import javax.media.j3d.BoundingPolytope;
import javax.media.j3d.BoundingSphere;
import javax.media.j3d.Transform3D;
import javax.vecmath.Matrix4d;
import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;
import javax.vecmath.Vector4d;
public class OBB implements Cloneable {
private ArrayList<Vector3d> vertexList = new ArrayList<Vector3d>();
private Vector4d[] plane;
private BoundingPolytope bp = null;
private static int edges[][] = { { 0, 1 }, { 0, 2 }, { 1, 3 }, { 2, 3 },
{ 0, 4 }, { 1, 5 }, { 3, 7 }, { 2, 6 },
{ 4, 5 }, { 4, 6 }, { 5, 7 }, { 6, 7 } };
private static Integer planes[][] = { {0, 2, 3, 1}, {0, 1, 5, 4},
{1, 3, 7, 5}, {4, 5, 7, 6},
{0, 4, 6, 2}, {2, 6, 7, 3}};
private static boolean[][] inside = new boolean[8][6];
public OBB() {
}
public OBB(Vector3d v1, Vector3d v2, Vector3d v3, Vector3d v4,
Vector3d v5, Vector3d v6, Vector3d v7, Vector3d v8) {
vertexList.add(v1);
vertexList.add(v2);
vertexList.add(v3);
vertexList.add(v4);
vertexList.add(v5);
vertexList.add(v6);
vertexList.add(v7);
vertexList.add(v8);
createPlanes();
}
public void addVertex(Vector3d v) {
vertexList.add(v);
}
public Vector3d getVertex(int i) {
return vertexList.get(i);
}
public BoundingPolytope getBoundingPolytope() {
return bp;
}
public BoundingSphere getBoundingSphere() {
double radius = 0.0;
Point3d p = new Point3d();
Vector3d cv = new Vector3d();
for (int i = 0; i < vertexList.size() - 1; i++) {
for (int j = i + 1; j < vertexList.size(); j++) {
Vector3d v = new Vector3d();
v.sub(vertexList.get(i), vertexList.get(j));
if (radius < v.length()) {
radius = v.length();
cv.add(vertexList.get(i), vertexList.get(j));
cv.scale(0.5);
p.x = cv.x;
p.y = cv.y;
p.z = cv.z;
}
}
}
BoundingSphere s = new BoundingSphere(p, radius / 2);
return s;
}
/**
* 頂点情報を元に面および境界多面体を生成する
*/
public void createPlanes() {
// 面の作成
Vector3d v1 = new Vector3d();
Vector3d v2 = new Vector3d();
Vector3d v3 = new Vector3d();
Vector4d[] plane = new Vector4d[6];
// 0231
v1 = getVertex(0);
v2.sub(getVertex(2), getVertex(0));
v3.sub(getVertex(1), getVertex(0));
Vector3d n = new Vector3d();
n.cross(v2, v3);
n.normalize();
plane[0] = new Vector4d();
plane[0].set(n.x, n.y, n.z, -n.dot(v1));
// 0154
v1 = getVertex(0);
v2.sub(getVertex(1), getVertex(0));
v3.sub(getVertex(4), getVertex(0));
n = new Vector3d();
n.cross(v2, v3);
n.normalize();
plane[1] = new Vector4d();
plane[1].set(n.x, n.y, n.z, -n.dot(v1));
// 1375
v1 = getVertex(1);
v2.sub(getVertex(3), getVertex(1));
v3.sub(getVertex(5), getVertex(1));
n = new Vector3d();
n.cross(v2, v3);
n.normalize();
plane[2] = new Vector4d();
plane[2].set(n.x, n.y, n.z, -n.dot(v1));
// 4576
v1 = getVertex(6);
v2.sub(getVertex(4), getVertex(6));
v3.sub(getVertex(7), getVertex(6));
n = new Vector3d();
n.cross(v2, v3);
n.normalize();
plane[3] = new Vector4d();
plane[3].set(n.x, n.y, n.z, -n.dot(v1));
// 0462
v1 = getVertex(6);
v2.sub(getVertex(2), getVertex(6));
v3.sub(getVertex(4), getVertex(6));
n = new Vector3d();
n.cross(v2, v3);
n.normalize();
plane[4] = new Vector4d();
plane[4].set(n.x, n.y, n.z, -n.dot(v1));
// 2673
v1 = getVertex(6);
v2.sub(getVertex(7), getVertex(6));
v3.sub(getVertex(2), getVertex(6));
n = new Vector3d();
n.cross(v2, v3);
n.normalize();
plane[5] = new Vector4d();
plane[5].set(n.x, n.y, n.z, -n.dot(v1));
this.plane = plane;
bp = new BoundingPolytope();
bp.setPlanes(plane);
}
/**
* 平面との衝突判定
* @param plane 衝突判定の対象となる平面
* @return 衝突判定の結果（衝突していない場合はnull）
*/
public CollisionResult intersect(Vector4d plane) {
int i = 0;
boolean inside = false;
boolean outside = false;
int count = 0;
Vector3d center = new Vector3d(0, 0, 0);
ArrayList<Vector3d> collisionPoints = new ArrayList<Vector3d>();
double l = Math.sqrt(plane.x * plane.x + plane.y * plane.y + plane.z * plane.z);
double length;
double deepest = 0.0;
Vector3d v;
for (i = 0; i < vertexList.size(); i++) {
v = vertexList.get(i);
if (GeometryUtility.inside(v, plane)) {
inside = true;
length = -(v.x * plane.x + v.y * plane.y + v.z * plane.z + plane.w) / l;
if (length > deepest + GeometryUtility.TOLERANCE) {
center.set(v);
collisionPoints.clear();
collisionPoints.add(v);
count = 1;
deepest = length;
} else if (length >= deepest - GeometryUtility.TOLERANCE) {
center.add(v);
collisionPoints.add(v);
count++;
}
} else {
outside = true;
}
}
if (!inside || !outside) {
// 全頂点が外側か全頂点が内側の場合
return null;
}
center.scale(1.0 / count);
CollisionResult cr = new CollisionResult();
cr.length = deepest;
cr.collisionPoint.setVector3d(center);
cr.collisionPoints = collisionPoints;
cr.normal.setX(plane.x / l);
cr.normal.setY(plane.y / l);
cr.normal.setZ(plane.z / l);
return cr;
}
/**
* OBBとの衝突判定
* @param another 衝突判定の対象となるOBB
* @return 衝突判定の結果（衝突していない場合はnull）
*/
public CollisionResult intersect(OBB another) {
// anotherの各頂点がthisの各面の内側にあるかを調べる
// i：anotherの頂点, j：thisの面
OBB o1 = this;
OBB o2 = another;
for (int i = 0; i < o2.vertexList.size(); i++) {
for (int j = 0; j < o1.plane.length; j++) {
inside[i][j] = GeometryUtility.inside(o2.vertexList.get(i), o1.plane[j]);
}
}
int v = 0;
boolean bCollides = false;
// anotherの各頂点がthisに包含されているか？
Vector3d contactCenter = new Vector3d();
int count = 0;
for (int i = 0; i < o2.vertexList.size(); i++) {
boolean f1 = false;
for (int p = 0; p < o1.plane.length; p++) {
f1 = inside[i][p];
if (!f1) break; // 包含されていない
}
if (f1) {
// thisのすべての面の内側 ＝ thisに包含されている
if (!bCollides) {
bCollides = true;
v = i;
}
contactCenter.add(o2.vertexList.get(i));
count++;
}
}
if (!bCollides) {
// thisの各頂点がanotherの各面の内側にあるかを調べる
o1 = another;
o2 = this;
for (int i = 0; i < o2.vertexList.size(); i++) {
for (int j = 0; j < o1.plane.length; j++) {
inside[i][j] = GeometryUtility.inside(o2.vertexList.get(i), o1.plane[j]);
}
}
// thisの各頂点がanotherに包含されているか？
for (int i = 0; i < o2.vertexList.size(); i++) {
boolean f1 = false;
for (int p = 0; p < o1.plane.length; p++) {
f1 = inside[i][p];
if (!f1) break; // 包含されていない
}
if (f1) {
// anotherのすべての面の内側 ＝ anotherに包含されている
if (!bCollides) {
bCollides = true;
v = i;
}
contactCenter.add(o2.vertexList.get(i));
count++;
}
}
}
CollisionResult cr = new CollisionResult();
ArrayList<Vector3d> collisionPoints = new ArrayList<Vector3d>();
if (bCollides) {
// いずれかのOBBの頂点vが他方のOBBに包含されていた場合(vを持つ側のOBBをo2、他方をo1とする)
// o1のどの面と衝突したかを判定する
double lMin = 0;
int p1 = -1;
Vector3d normal = null;
contactCenter.scale(1.0 / (double)count);
for (int p = 0; p < o1.plane.length; p++) {
Vector3d n = new Vector3d(o1.plane[p].x, o1.plane[p].y, o1.plane[p].z);
double l = -(contactCenter.x * o1.plane[p].x + contactCenter.y * o1.plane[p].y +
contactCenter.z * o1.plane[p].z + o1.plane[p].w) / n.length();
if (lMin > l || normal == null) {
lMin = l;
p1 = p;
normal = n;
}
}
// o2上のvを含む辺のうち衝突面の法線と逆方向に向かっている辺を探す(そのような辺の数によって接触状況が変わる)
Vector3d collisionPoint = o2.vertexList.get(v);
Vector3d d1;
ArrayList<Integer> contactEdges = new ArrayList<Integer>();
for (int e = 0; e < edges.length; e++) {
if (v == edges[e][0]) {
d1 = new Vector3d(o2.vertexList.get(edges[e][1]));
d1.sub(collisionPoint);
if (normal.dot(d1) < GeometryUtility.TOLERANCE) {
// 衝突面に接している辺
contactEdges.add(edges[e][1]); // vの反対側の端点
}
} else if (v == edges[e][1]) {
d1 = new Vector3d(o2.vertexList.get(edges[e][0]));
d1.sub(collisionPoint);
if (normal.dot(d1) < GeometryUtility.TOLERANCE) {
// 衝突面に接している辺
contactEdges.add(edges[e][0]); // vの反対側の端点
}
}
}
// 接触状況に応じた処理
if (contactEdges.size() == 0) {
// a) 頂点で接触
collisionPoints.add(collisionPoint);
cr.collisionPoint.setVector3d(collisionPoint);
} else if (contactEdges.size() == 1) {
// b) 辺で接触
boolean f1 = true;
Vector3d otherPoint = o2.vertexList.get(contactEdges.get(0));
Vector3d intersectionPoint = null;
double nearest = 0.0;
for (int p = 0; p < o1.plane.length; p++) {
if (!inside[contactEdges.get(0)][p]) {
// collisionPoint と otherPoint の間を通る平面
Vector3d ip = GeometryUtility.intersect(o1.plane[p], collisionPoint, otherPoint);
if (ip != null) {
f1 = false; // 包含されていない
Vector3d ip2 = (Vector3d)ip.clone();
ip2.sub(collisionPoint);
double d = ip2.length();
if (intersectionPoint == null || d < nearest) {
intersectionPoint = ip;
nearest = d;
}
}
}
}
if (f1) {
// b-1) 辺全体が接触している
collisionPoints.add(collisionPoint);
collisionPoints.add(otherPoint);
Vector3d center = new Vector3d(collisionPoint);
center.add(otherPoint);
center.scale(0.5);
cr.collisionPoint.setVector3d(center);
} else {
// b-2) 辺の一部が接触している
collisionPoints.add(collisionPoint);
collisionPoints.add(intersectionPoint);
Vector3d center = new Vector3d(collisionPoint);
center.add(intersectionPoint);
center.scale(0.5);
cr.collisionPoint.setVector3d(center);
}
} else {
// c) 面で接触
Vector3d center = new Vector3d();
int v2 = contactEdges.get(0);
int v3 = contactEdges.get(1);
int p2;
for (p2 = 0; p2 < o2.plane.length; p2++) {
if (v == planes[p2][0] || v == planes[p2][1] || v == planes[p2][2] || v == planes[p2][3]) {
if (v2 == planes[p2][0] || v2 == planes[p2][1] || v2 == planes[p2][2] || v2 == planes[p2][3]) {
if (v3 == planes[p2][0] || v3 == planes[p2][1] || v3 == planes[p2][2] || v3 == planes[p2][3]) {
break;
}
}
}
}
// o1の面p1とo2の面p2が接している
// 面と面の共通領域を求める
for (v2 = 0; v2 < 4; v2++) {
if (planes[p2][v2] == v) break;
}
Vector3d d2 = (Vector3d)o2.vertexList.get(planes[p2][(v2 + 1) % 4]).clone();
d2.sub(collisionPoint);
d2.cross(normal, d2);
int sign = -1;
int v1;
for (v1 = 4; v1 >= 0; v1--) {
d1 = (Vector3d)o1.vertexList.get(planes[p1][v1 % 4]).clone();
d1.sub(o1.vertexList.get(planes[p1][(v1 + 1) % 4]));
if (d2.dot(d1) > -GeometryUtility.TOLERANCE) {
sign = 1;
} else if (sign == 1) {
// 内積の符号が正から負に変わったとき
v1 = (v1 + 1) % 4;
break;
}
}
Vector3d vp1, vp2, dst1, dst2, src1, src2, d3;
ArrayList<Vector3d[]> contours = new ArrayList<Vector3d[]>();
for (int i = 0; i < 4; i++) {
dst1 = vp1 = (Vector3d)o1.vertexList.get(planes[p1][(v1 - i + 4) % 4]).clone();
src2 = vp2 = (Vector3d)o2.vertexList.get(planes[p2][(v2 + i) % 4]).clone();
src1 = (Vector3d)o1.vertexList.get(planes[p1][(v1 - i + 4 + 1) % 4]).clone();
dst2 = (Vector3d)o2.vertexList.get(planes[p2][(v2 + i + 1) % 4]).clone();
d1 = (Vector3d)dst1.clone();
d1.sub(src1);
d2 = (Vector3d)dst2.clone();
d2.sub(src2);
d3 = (Vector3d)vp2.clone();
d3.sub(vp1);
d1.cross(d1, normal);
if (d1.dot(d3) < GeometryUtility.TOLERANCE) {
// o1の辺<src1,dst1>は、頂点vp2の内側にあるので、共通領域の輪郭を構成する
contours.add(new Vector3d[]{src1, dst1});
}
d3.negate();
d2.cross(d2, normal);
if (d2.dot(d3) < GeometryUtility.TOLERANCE) {
// o2の辺<src2,dst2>は、頂点vp1の内側にあるので、共通領域の輪郭を構成する
contours.add(new Vector3d[]{src2, dst2});
}
}
Vector3d[] edge1, edge2;
Vector3d d;
Vector4d p;
for (int i = 0; i < contours.size(); i++) {
edge1 = contours.get(i);
edge2 = contours.get((i + 1) % contours.size());
if (edge1[1].equals(edge2[0])) {
// edge1とedge2は始点と終点を共有している
if (!collisionPoints.contains(edge1[1])) {
collisionPoints.add(edge1[1]);
center.add(edge1[1]);
}
} else if (!edge1[0].equals(edge2[0]) && !edge1[1].equals(edge2[1])) {
// edge1とedge2の交点を求める
d = (Vector3d)edge2[1].clone();
d.add(normal);
p = GeometryUtility.createPlane(edge2[0], edge2[1], d);
if (p != null) {
d = GeometryUtility.intersect(p, edge1[0], edge1[1]);
if (d != null && !collisionPoints.contains(d)) {
collisionPoints.add(d);
center.add(d);
}
}
}
}
if (collisionPoints.size() <= 2) return null; // 面で接触しているはずが2点以下である場合、衝突していない
center.scale(1.0 / (double)collisionPoints.size());
cr.collisionPoint.setVector3d(center);
}
cr.length = lMin;
cr.collisionPoints = collisionPoints;
if (o2 == this) normal.negate();
normal.normalize();
cr.normal = normal;
return cr;
} else {
// anotherの辺がthisの面と交わっているか？
Vector3d center = new Vector3d();
Vector3d normal = new Vector3d();
ArrayList<Vector3d> intersections = new ArrayList<Vector3d>();
ArrayList<Integer> contactPlanes = new ArrayList<Integer>();
for (int e = 0; e < edges.length; e++) {
// anotherの各辺eに対して処理
Vector3d intersection = null;
intersections.clear();
contactPlanes.clear();
for (int p = 0; p < this.plane.length; p++) {
if (inside[edges[e][0]][p] == !inside[edges[e][1]][p]) {
// anotherの辺eが、thisの面pを貫いている
Vector3d ip = GeometryUtility.intersect(
this.plane[p],
another.vertexList.get(edges[e][0]),
another.vertexList.get(edges[e][1]));
// eとpの交点がthisに包含されているか?
if (ip != null) {
intersection = ip;
boolean bInside = true;
for (int p2 = 0; p2 < 6; p2++) {
if (p != p2
&& !GeometryUtility.inside(intersection, this.plane[p2])) {
bInside = false;
break;
}
}
if (bInside) {
// eがpと交わっている
contactPlanes.add(p);
intersections.add(intersection);
bCollides = true;
}
}
}
}
if (contactPlanes.size() == 1) {
// eと交わっているthisの面が1つ
collisionPoints.add(intersection);
center.add(intersection);
} else if (contactPlanes.size() == 2) {
// eと交わっているthisの面が2つ
int p1 = contactPlanes.get(0);
int p2 = contactPlanes.get(1);
ArrayList<Integer> p1vs = new ArrayList<Integer>(Arrays.asList(planes[p1]));
ArrayList<Integer> p2vs = new ArrayList<Integer>(Arrays.asList(planes[p2]));
p1vs.retainAll(p2vs);
if (p1vs.size() == 2) {
// p1とp2は隣り合う面(anotherの辺eはthisのある辺と接触している)
Vector3d v1 = this.vertexList.get(p1vs.get(0)); // p1とp2の間の辺の始点
Vector3d v2 = this.vertexList.get(p1vs.get(1)); // p1とp2の間の辺の終点
Vector3d d1 = (Vector3d)v1.clone();
d1.sub(v2);
Vector3d d2 = (Vector3d)another.vertexList.get(edges[e][0]).clone();
d2.sub(another.vertexList.get(edges[e][1]));
normal.cross(d1, d2); // 辺eと、p1とp2の間の辺の両方に垂直なベクトルを法線方向とする
normal.normalize();
Vector3d n1 = new Vector3d(this.plane[p1].x, this.plane[p1].y, this.plane[p1].z);
Vector3d n2 = new Vector3d(this.plane[p2].x, this.plane[p2].y, this.plane[p2].z);
n1.normalize();
n2.normalize();
n1.add(n2);
n1.scale(0.5);
if (n1.dot(normal) < -GeometryUtility.TOLERANCE) {
normal.negate();
}
intersection = intersections.get(0);
intersection.add(intersections.get(1));
intersection.scale(0.5);
center.add(intersection); // 衝突点は1つ
collisionPoints.add(intersection);
Vector3d c = GeometryUtility.nearest(v1, v2, center);
c.sub(center);
cr.length = c.length();
cr.normal = normal;
} else {
// p1とp2は向かい合う面(anotherの辺eはthisのある面と接触している)
center.add(intersections.get(0)); // 衝突点は2つ
center.add(intersections.get(1));
collisionPoints.add(intersections.get(0));
collisionPoints.add(intersections.get(1));
// thisのどの面と衝突したかを判定する
double lMin = 0;
normal = null;
Vector4d plane;
for (int p = 0; p < this.plane.length; p++) {
plane = this.plane[p];
Vector3d n = new Vector3d(plane.x, plane.y, plane.z);
double l = -(center.x * plane.x + center.y * plane.y + center.z * plane.z + plane.w) / n.length();
if (lMin > l || normal == null) {
lMin = l;
normal = n;
}
}
normal.normalize();
cr.length = lMin;
cr.normal = normal;
}
}
}
if (!bCollides) return null;
center.scale(1.0 / (double)collisionPoints.size());
cr.collisionPoint.setVector3d(center);
cr.collisionPoints = collisionPoints;
return cr;
}
}
public Object clone() {
OBB obb = new OBB();
obb.plane = new Vector4d[6];
for (int i = 0; i < plane.length; i++) {
obb.plane[i] = (Vector4d) plane[i].clone();
}
for (int i = 0; i < vertexList.size(); i++) {
obb.vertexList.add((Vector3d) vertexList.get(i).clone());
}
obb.bp = new BoundingPolytope(obb.plane);
// System.out.println("veretexListSIZE:"+vertexList.size());
return obb;
}
public void transform(Transform3D t) {
// TODO Auto-generated method stub
bp.transform(t);
bp.getPlanes(plane);
// for(int i = 0; i<plane.length;i++){
// System.out.println("plane");
// System.out.print(plane[i].x + "," + plane[i].y + "," + plane[i].z +
// "," + plane[i].w);
// t.transform(plane[i]);
// System.out.println("-->" + plane[i].x + "," + plane[i].y + "," +
// plane[i].z + "," + plane[i].w);
// }
for (int i = 0; i < vertexList.size(); i++) {
// System.out.println("vertex");
// System.out.print(vertexList.get(i).x + "," + vertexList.get(i).y
// + "," + vertexList.get(i).z);
Matrix4d mat4d = new Matrix4d();
t.get(mat4d);
double x = mat4d.m00 * vertexList.get(i).x + mat4d.m01
* vertexList.get(i).y + mat4d.m02 * vertexList.get(i).z
+ mat4d.m03;
double y = mat4d.m10 * vertexList.get(i).x + mat4d.m11
* vertexList.get(i).y + mat4d.m12 * vertexList.get(i).z
+ mat4d.m13;
double z = mat4d.m20 * vertexList.get(i).x + mat4d.m21
* vertexList.get(i).y + mat4d.m22 * vertexList.get(i).z
+ mat4d.m23;
vertexList.get(i).x = x;
vertexList.get(i).y = y;
vertexList.get(i).z = z;
// t.transform(vertexList.get(i));
// System.out.println("-->" + vertexList.get(i).x + "," +
// vertexList.get(i).y + "," + vertexList.get(i).z);
}
}
}
