private void stepSimplicies23() {
if ((count2 < complex.getSimplicies()[2].getSize())
&& (count3 < complex.getSimplicies()[3].getSize())) {
Simplex simplex2 = (Simplex) complex.getSimplicies()[2].get(count2);
Simplex simplex3 = (Simplex) complex.getSimplicies()[3].get(count3);
if (simplex3.getAlpha() < simplex2.getAlpha()) {
addTetrahedron(simplex3, Colors.yellow);
System.out.println(count3 + ": alpha3 = " + simplex3.getAlpha());
count3++;
} else {
System.out.println(count2 + ": alpha2 = " + simplex2.getAlpha());
addTriangle(simplex2, Colors.yellow);
count2++;
}
} else {
if (count2 < complex.getSimplicies()[2].getSize()) {
Simplex simplex2 = (Simplex) complex.getSimplicies()[2].get(count2);
System.out.println(count2 + ": alpha2 = " + simplex2.getAlpha());
addTriangle(simplex2, Colors.yellow);
count2++;
} else {
if (count3 < complex.getSimplicies()[3].getSize()) {
Simplex simplex3 = (Simplex) complex.getSimplicies()[3].get(count3);
addTetrahedron(simplex3, Colors.yellow);
System.out.println(count3 + ": alpha3 = " + simplex3.getAlpha());
count3++;
}
}
}
}
public static void test(double[][] A, double[] b, double[] c) {
Simplex lp = new Simplex(A, b, c);
System.out.println("value = " + lp.value());
double[] x = lp.primal();
for (int i = 0; i < x.length; i++) System.out.println("x[" + i + "] = " + x[i]);
double[] y = lp.dual();
for (int j = 0; j < y.length; j++) System.out.println("y[" + j + "] = " + y[j]);
}
private void stepSimplicies3() {
if (count3 < complex.getSimplicies()[3].getSize()) {
Simplex simplex3 = (Simplex) complex.getSimplicies()[3].get(count3);
addTetrahedron(simplex3, Colors.yellow);
System.out.println(count3 + ": alpha3 = " + simplex3.getAlpha());
count3++;
}
}
public BranchGroup addTriangle(Simplex simplex, Color3f color) {
int[] p = simplex.getPoints();
BranchGroup bgTriangle = new BranchGroup();
bgTriangle.setCapability(BranchGroup.ALLOW_DETACH);
javax.vecmath.Point3d[] coords = new javax.vecmath.Point3d[6];
TriangleArray ta = new TriangleArray(6, TriangleArray.COORDINATES | TriangleArray.COLOR_3);
Point3d p0 = (Point3d) complex.getPoints().get(simplex.getPoints()[0]);
Point3d p1 = (Point3d) complex.getPoints().get(simplex.getPoints()[1]);
Point3d p2 = (Point3d) complex.getPoints().get(simplex.getPoints()[2]);
coords[0] = new javax.vecmath.Point3d(p0.getX(), p0.getY(), p0.getZ());
coords[1] = new javax.vecmath.Point3d(p1.getX(), p1.getY(), p1.getZ());
coords[2] = new javax.vecmath.Point3d(p2.getX(), p2.getY(), p2.getZ());
coords[3] = new javax.vecmath.Point3d(p0.getX(), p0.getY(), p0.getZ());
coords[4] = new javax.vecmath.Point3d(p2.getX(), p2.getY(), p2.getZ());
coords[5] = new javax.vecmath.Point3d(p1.getX(), p1.getY(), p1.getZ());
ta.setCoordinates(0, coords);
Color3f[] colors = new Color3f[6];
for (int i = 0; i < 3; i++) colors[i] = Colors.black;
for (int i = 3; i < 6; i++) colors[i] = Colors.yellow;
ta.setColors(0, colors);
Appearance ap = new Appearance();
ap.setCapability(Appearance.ALLOW_LINE_ATTRIBUTES_READ);
ap.setCapability(Appearance.ALLOW_LINE_ATTRIBUTES_WRITE);
ap.setCapability(Appearance.ALLOW_COLORING_ATTRIBUTES_READ);
ap.setCapability(Appearance.ALLOW_COLORING_ATTRIBUTES_WRITE);
ap.setLineAttributes(new LineAttributes(2, LineAttributes.PATTERN_SOLID, false));
ap.setMaterial(new Material());
ap.setTransparencyAttributes(
new TransparencyAttributes(TransparencyAttributes.SCREEN_DOOR, 0.5f));
Shape3D shape = new Shape3D(ta, ap);
shape.setCapability(Shape3D.ALLOW_APPEARANCE_READ);
shape.setCapability(Shape3D.ALLOW_APPEARANCE_WRITE);
simplex.setTriangle(shape);
// simplex.bgTriangle = bgTriangle;
TransformGroup tgTriangle = new TransformGroup();
tgTriangle.addChild(shape);
bgTriangle.addChild(tgTriangle);
spin.addChild(bgTriangle);
return bgTriangle;
}
public void addSphere(Simplex simplex, double radius, Color3f color) {
geom3d.Point3d p = (Point3d) complex.getPoints().get(simplex.getPoints()[0]);
BranchGroup bgSphere = new BranchGroup();
bgSphere.setCapability(BranchGroup.ALLOW_DETACH);
Appearance ap = new Appearance();
ap.setCapability(Appearance.ALLOW_COLORING_ATTRIBUTES_READ);
ap.setCapability(Appearance.ALLOW_COLORING_ATTRIBUTES_WRITE);
ap.setColoringAttributes(new ColoringAttributes(color, ColoringAttributes.NICEST));
simplex.setSphere(new Sphere(new Float(radius), ap));
simplex.getSphere().getShape().setCapability(Shape3D.ALLOW_APPEARANCE_READ);
simplex.getSphere().getShape().setCapability(Shape3D.ALLOW_APPEARANCE_WRITE);
Transform3D tr = new Transform3D();
tr.setTranslation(new javax.vecmath.Vector3d(p.getX(), p.getY(), p.getZ()));
TransformGroup tgSphere = new TransformGroup(tr);
tgSphere.addChild(simplex.getSphere());
bgSphere.addChild(tgSphere);
spin.addChild(bgSphere);
}
// test client
public static void main(String[] args) {
try {
test1();
} catch (ArithmeticException e) {
e.printStackTrace();
}
System.out.println("--------------------------------");
try {
test2();
} catch (ArithmeticException e) {
e.printStackTrace();
}
System.out.println("--------------------------------");
try {
test3();
} catch (ArithmeticException e) {
e.printStackTrace();
}
System.out.println("--------------------------------");
try {
test4();
} catch (ArithmeticException e) {
e.printStackTrace();
}
System.out.println("--------------------------------");
int M = Integer.parseInt(args[0]);
int N = Integer.parseInt(args[1]);
double[] c = new double[N];
double[] b = new double[M];
double[][] A = new double[M][N];
for (int j = 0; j < N; j++) c[j] = Math.random() * 1000;
for (int i = 0; i < M; i++) b[i] = Math.random() * 1000;
for (int i = 0; i < M; i++) for (int j = 0; j < N; j++) A[i][j] = Math.random() * 1000;
Simplex lp = new Simplex(A, b, c);
System.out.println(lp.value());
}
/**
* Tests some exceptional behavior.
*
* @exception IndexOutOfBoundsException
* @see java.lang.System#getProperty(java.lang.String)
* @see SecurityManager#checkPermission
*/
@Test(expected = IllegalArgumentException.class)
@SuppressWarnings("unused")
public void testForException() {
Simplex sloser0 = Simplex.getSimplex(new int[] {-1, Integer.MAX_VALUE});
Simplex sloser1 = Simplex.getSimplex(new int[] {1, 1});
}
public BranchGroup addTetrahedron(Simplex simplex, Color3f color) {
BranchGroup bgTetrahedron = new BranchGroup();
bgTetrahedron.setCapability(BranchGroup.ALLOW_DETACH);
javax.vecmath.Point3d[] coords = new javax.vecmath.Point3d[12];
LineArray la = new LineArray(12, LineArray.COORDINATES | LineArray.COLOR_3);
Point3d p0 = (Point3d) complex.getPoints().get(simplex.getPoints()[0]);
Point3d p1 = (Point3d) complex.getPoints().get(simplex.getPoints()[1]);
Point3d p2 = (Point3d) complex.getPoints().get(simplex.getPoints()[2]);
Point3d p3 = (Point3d) complex.getPoints().get(simplex.getPoints()[3]);
coords[0] = coords[2] = coords[4] = new javax.vecmath.Point3d(p0.getX(), p0.getY(), p0.getZ());
coords[1] = coords[6] = coords[8] = new javax.vecmath.Point3d(p1.getX(), p1.getY(), p1.getZ());
coords[3] = coords[7] = coords[10] = new javax.vecmath.Point3d(p2.getX(), p2.getY(), p2.getZ());
coords[5] = coords[9] = coords[11] = new javax.vecmath.Point3d(p3.getX(), p3.getY(), p3.getZ());
la.setCoordinates(0, coords);
Color3f[] colors = new Color3f[12];
for (int i = 0; i < 12; i++) colors[i] = Colors.black;
la.setColors(0, colors);
javax.vecmath.Point3d[] coordsTr = new javax.vecmath.Point3d[24];
TriangleArray tr = new TriangleArray(24, TriangleArray.COORDINATES | TriangleArray.COLOR_3);
coordsTr[0] =
coordsTr[3] = coordsTr[6] = new javax.vecmath.Point3d(p0.getX(), p0.getY(), p0.getZ());
coordsTr[1] =
coordsTr[4] = coordsTr[9] = new javax.vecmath.Point3d(p1.getX(), p1.getY(), p1.getZ());
coordsTr[2] =
coordsTr[7] = coordsTr[10] = new javax.vecmath.Point3d(p2.getX(), p2.getY(), p2.getZ());
coordsTr[5] =
coordsTr[8] = coordsTr[11] = new javax.vecmath.Point3d(p3.getX(), p3.getY(), p3.getZ());
coordsTr[12] =
coordsTr[15] = coordsTr[18] = new javax.vecmath.Point3d(p0.getX(), p0.getY(), p0.getZ());
coordsTr[14] =
coordsTr[17] = coordsTr[21] = new javax.vecmath.Point3d(p1.getX(), p1.getY(), p1.getZ());
coordsTr[13] =
coordsTr[20] = coordsTr[23] = new javax.vecmath.Point3d(p2.getX(), p2.getY(), p2.getZ());
coordsTr[16] =
coordsTr[19] = coordsTr[22] = new javax.vecmath.Point3d(p3.getX(), p3.getY(), p3.getZ());
tr.setCoordinates(0, coordsTr);
Color3f[] colorsTr = new Color3f[24];
for (int i = 0; i < 24; i++) colorsTr[i] = Colors.green;
tr.setColors(0, colorsTr);
Appearance ap = new Appearance();
ap.setCapability(Appearance.ALLOW_LINE_ATTRIBUTES_READ);
ap.setCapability(Appearance.ALLOW_LINE_ATTRIBUTES_WRITE);
ap.setCapability(Appearance.ALLOW_COLORING_ATTRIBUTES_READ);
ap.setCapability(Appearance.ALLOW_COLORING_ATTRIBUTES_WRITE);
// ap.setCapability(Appearance.ALLOW_MATERIAL_READ);
// ap.setCapability(Appearance.ALLOW_MATERIAL_WRITE);
ap.setLineAttributes(new LineAttributes(2, LineAttributes.PATTERN_SOLID, false));
ap.setMaterial(new Material());
// ap.setTransparencyAttributes(new
// TransparencyAttributes(TransparencyAttributes.SCREEN_DOOR, 0.5f));
// LineArray la = cell.getLineArray();
Shape3D shape = new Shape3D(la, ap);
Shape3D shapeTr = new Shape3D(tr, ap);
shape.setCapability(Shape3D.ALLOW_APPEARANCE_READ);
shape.setCapability(Shape3D.ALLOW_APPEARANCE_WRITE);
simplex.setTetrahedron(shape);
// cell.setLineArray(la);
simplex.setTriangleArray(tr);
simplex.setBgTetrahedron(bgTetrahedron);
TransformGroup tgTetrahedron = new TransformGroup();
tgTetrahedron.addChild(shape);
tgTetrahedron.addChild(shapeTr);
bgTetrahedron.addChild(tgTetrahedron);
spin.addChild(bgTetrahedron);
return bgTetrahedron;
}
/**
* Compute the closest points between two shapes. Supports any combination of: CircleShape and
* PolygonShape. The simplex cache is input/output. On the first call set SimplexCache.count to
* zero.
*
* @param output
* @param cache
* @param input
*/
public final void distance(
final DistanceOutput output, final SimplexCache cache, final DistanceInput input) {
GJK_CALLS++;
final DistanceProxy proxyA = input.proxyA;
final DistanceProxy proxyB = input.proxyB;
Transform transformA = input.transformA;
Transform transformB = input.transformB;
// Initialize the simplex.
simplex.readCache(cache, proxyA, transformA, proxyB, transformB);
// Get simplex vertices as an array.
SimplexVertex[] vertices = simplex.vertices;
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
// (pooled above)
int saveCount = 0;
simplex.getClosestPoint(closestPoint);
float distanceSqr1 = closestPoint.lengthSquared();
float distanceSqr2 = distanceSqr1;
// Main iteration loop
int iter = 0;
while (iter < MAX_ITERS) {
// Copy simplex so we can identify duplicates.
saveCount = simplex.m_count;
for (int i = 0; i < saveCount; i++) {
saveA[i] = vertices[i].indexA;
saveB[i] = vertices[i].indexB;
}
switch (simplex.m_count) {
case 1:
break;
case 2:
simplex.solve2();
break;
case 3:
simplex.solve3();
break;
default:
assert (false);
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex.m_count == 3) {
break;
}
// Compute closest point.
simplex.getClosestPoint(closestPoint);
distanceSqr2 = closestPoint.lengthSquared();
// ensure progress
if (distanceSqr2 >= distanceSqr1) {
// break;
}
distanceSqr1 = distanceSqr2;
// get search direction;
simplex.getSearchDirection(d);
// Ensure the search direction is numerically fit.
if (d.lengthSquared() < Settings.EPSILON * Settings.EPSILON) {
// The origin is probably contained by a line segment
// or triangle. Thus the shapes are overlapped.
// We can't return zero here even though there may be overlap.
// In case the simplex is a point, segment, or triangle it is difficult
// to determine if the origin is contained in the CSO or very close to it.
break;
}
/*
* SimplexVertex* vertex = vertices + simplex.m_count; vertex.indexA =
* proxyA.GetSupport(MulT(transformA.R, -d)); vertex.wA = Mul(transformA,
* proxyA.GetVertex(vertex.indexA)); Vec2 wBLocal; vertex.indexB =
* proxyB.GetSupport(MulT(transformB.R, d)); vertex.wB = Mul(transformB,
* proxyB.GetVertex(vertex.indexB)); vertex.w = vertex.wB - vertex.wA;
*/
// Compute a tentative new simplex vertex using support points.
SimplexVertex vertex = vertices[simplex.m_count];
Rot.mulTransUnsafe(transformA.q, d.negateLocal(), temp);
vertex.indexA = proxyA.getSupport(temp);
Transform.mulToOutUnsafe(transformA, proxyA.getVertex(vertex.indexA), vertex.wA);
// Vec2 wBLocal;
Rot.mulTransUnsafe(transformB.q, d.negateLocal(), temp);
vertex.indexB = proxyB.getSupport(temp);
Transform.mulToOutUnsafe(transformB, proxyB.getVertex(vertex.indexB), vertex.wB);
vertex.w.set(vertex.wB).subLocal(vertex.wA);
// Iteration count is equated to the number of support point calls.
++iter;
++GJK_ITERS;
// Check for duplicate support points. This is the main termination criteria.
boolean duplicate = false;
for (int i = 0; i < saveCount; ++i) {
if (vertex.indexA == saveA[i] && vertex.indexB == saveB[i]) {
duplicate = true;
break;
}
}
// If we found a duplicate support point we must exit to avoid cycling.
if (duplicate) {
break;
}
// New vertex is ok and needed.
++simplex.m_count;
}
GJK_MAX_ITERS = MathUtils.max(GJK_MAX_ITERS, iter);
// Prepare output.
simplex.getWitnessPoints(output.pointA, output.pointB);
output.distance = MathUtils.distance(output.pointA, output.pointB);
output.iterations = iter;
// Cache the simplex.
simplex.writeCache(cache);
// Apply radii if requested.
if (input.useRadii) {
float rA = proxyA.m_radius;
float rB = proxyB.m_radius;
if (output.distance > rA + rB && output.distance > Settings.EPSILON) {
// Shapes are still no overlapped.
// Move the witness points to the outer surface.
output.distance -= rA + rB;
normal.set(output.pointB).subLocal(output.pointA);
normal.normalize();
temp.set(normal).mulLocal(rA);
output.pointA.addLocal(temp);
temp.set(normal).mulLocal(rB);
output.pointB.subLocal(temp);
} else {
// Shapes are overlapped when radii are considered.
// Move the witness points to the middle.
// Vec2 p = 0.5f * (output.pointA + output.pointB);
output.pointA.addLocal(output.pointB).mulLocal(.5f);
output.pointB.set(output.pointA);
output.distance = 0.0f;
}
}
}
