Permalink
Browse files

Merge pull request #51 from ppiastucki/convex_trimesh_improvements

Improve trimesh vs convex collisions for concave trimeshes
  • Loading branch information...
tzaeschke committed Apr 2, 2017
2 parents 759b20c + 1653a23 commit 170876f4edbf362a53a0fb620e61bd7416303251
@@ -137,7 +137,7 @@ void build(
//
// /** Preprocess the trimesh data to remove mark unnecessary edges and vertices */
// //ODE_API
// void preprocess(DTriMeshData g);
void preprocess();
// /** Get and set the internal preprocessed trimesh data buffer, for loading and saving */
// //ODE_API
// //void dGeomTriMeshDataGetBuffer(dTriMeshData g, unsigned char** buf, int* bufLen) {
@@ -1,5 +1,7 @@
package org.ode4j.ode.internal;
import java.util.Arrays;
import org.ode4j.ode.DAABBC;
import org.ode4j.ode.DColliderFn;
import org.ode4j.ode.DContactGeomBuffer;
@@ -30,7 +32,7 @@ public int dColliderFn(DGeom o1, DGeom o2, int flags, DContactGeomBuffer contact
ptrimesh.getAabbSet().gim_aabbset_box_collision(test_aabb, collision_result);
int contactcount = 0;
if (collision_result.size() != 0) {
int[] boxesresult = collision_result.GIM_DYNARRAY_POINTER();
int[] boxesresult = Arrays.copyOf(collision_result.GIM_DYNARRAY_POINTER(), collision_result.size());
ptrimesh.gim_trimesh_locks_work_data();
CollideConvexTrimeshTrianglesCCD collideFn = new CollideConvexTrimeshTrianglesCCD();
contactcount = collideFn.collide(o1, o2, boxesresult, flags, contacts);
@@ -489,10 +489,6 @@ public int dColliderFn(DGeom o1, DGeom o2, int flags,
@Override
public void run(Object obj, ccd_vec3_t _dir, ccd_vec3_t v) {
final ccd_triangle_t o = (ccd_triangle_t) obj;
// Not needed as trimesh already returns transformed vertices
// final ccd_vec3_t dir = new ccd_vec3_t();
// ccdVec3Copy(dir, _dir);
// ccdQuatRotVec(dir, o.rot_inv);
double maxdot = -CCD_REAL_MAX;
for (int i = 0; i < 3; i++) {
double dot = ccdVec3Dot(_dir, o.vertices[i]);
@@ -501,14 +497,11 @@ public void run(Object obj, ccd_vec3_t _dir, ccd_vec3_t v) {
maxdot = dot;
}
}
// ccdQuatRotVec(v, o.rot);
// ccdVec3Add(v, o.pos);
}
};
private static final float CONTACT_DEPTH_EPSILON = 0.0001f;
private static final float CONTACT_POS_EPSILON = 0.0001f;
private static final float CONTACT_PERTURBATION_ANGLE = 0.001f;
private static final float CONTACT_POS_EPSILON = 0.001f;
private static final float CONTACT_PERTURBATION_ANGLE = 0.005f;
public static class CollideConvexTrimeshTrianglesCCD {
public int collide(DGeom o1, DGeom o2, int[] triindices, int flags, DContactGeomBuffer contacts) {
@@ -525,41 +518,145 @@ public int collide(DGeom o1, DGeom o2, int[] triindices, int flags, DContactGeom
for (int j = 0; j < 3; j++) {
c2.vertices[j].set(triangle[j].get0(), triangle[j].get1(), triangle[j].get2());
}
setObjPosToTriangleCenter(c2);
if (ccdCollide(o1, o2, flags, tempContacts, c1, ccdSupportConvex, ccdCenter, c2, ccdSupportTriangle,
ccdCenter) == 1) {
DContactGeom c = tempContacts.get();
c.side2 = i;
contactcount = addUniqueContact(contacts, c, contactcount, maxcontacts);
if ((flags & OdeConstants.CONTACTS_UNIMPORTANT) != 0) {
break;
DContactGeom tempContact = tempContacts.get();
tempContact.side2 = i;
if (correctContactNormal(c2, tempContact, triangle, o2, triindices)) {
contactcount = addUniqueContact(contacts, tempContact, contactcount, maxcontacts);
if ((flags & OdeConstants.CONTACTS_UNIMPORTANT) != 0) {
break;
}
}
}
}
if (contactcount == 1 && (flags & OdeConstants.CONTACTS_UNIMPORTANT) == 0) {
DContactGeom contact = contacts.get(0);
((DxTriMesh) o2).FetchTransformedTriangle(contact.side2, triangle);
contactcount = addPerturbedContacts(o1, o2, flags, c1, c2, contacts, triangle, contact, contactcount);
contactcount = addPerturbedContacts(o1, o2, flags, c1, c2, contacts, triangle, contact, contactcount,
triindices);
// Normalize accumulated normals
for (int i = 0; i < contactcount; i++) {
contact = contacts.get(i);
contact.normal.safeNormalize();
}
}
return contactcount;
}
private void setObjPosToTriangleCenter(ccd_triangle_t t) {
ccdVec3Set(t.pos, 0, 0, 0);
for (int j = 0; j < 3; j++) {
ccdVec3Add(t.pos, t.vertices[j]);
}
ccdVec3Scale(t.pos, 1.0f / 3.0f);
}
private boolean correctContactNormal(ccd_triangle_t t, DContactGeom contact, DVector3[] triangle, DGeom geom,
int[] indices) {
ccd_vec3_t edge1 = new ccd_vec3_t();
ccd_vec3_t edge2 = new ccd_vec3_t();
ccdVec3Copy(edge1, t.vertices[1]);
ccdVec3Sub(edge1, t.vertices[0]);
ccdVec3Copy(edge2, t.vertices[2]);
ccdVec3Sub(edge2, t.vertices[1]);
ccd_vec3_t triangleNormal = new ccd_vec3_t();
ccdVec3Cross(triangleNormal, edge1, edge2);
// Triangle face normal
if (!ccdVec3Normalize(triangleNormal)) {
return false;
}
ccd_vec3_t contactNormal = new ccd_vec3_t();
ccdVec3Set(contactNormal, contact.normal);
ccd_vec3_t scaledContactNormal = new ccd_vec3_t();
ccdVec3Copy(scaledContactNormal, contactNormal);
ccdVec3Scale(scaledContactNormal, contact.depth);
boolean edgeContact = false;
// Check the edges to see if one of them is involved
for (int i = 0; i < 3; i++) {
ccd_vec3_t edgeAxis = new ccd_vec3_t();
ccdVec3Copy(edgeAxis, t.vertices[(i + 1) % 3]);
ccdVec3Sub(edgeAxis, t.vertices[i]);
// Edge axis
if (!ccdVec3Normalize(edgeAxis)) {
return false;
}
// Edge Normal
ccd_vec3_t edgeNormal = new ccd_vec3_t();
ccdVec3Cross(edgeNormal, edgeAxis, triangleNormal);
ccdVec3Normalize(edgeNormal);
// Check if the contact point is located close to any edge -
// move it
// back and forth and check the resulting segment for
// intersection
// with the edge plane
ccd_vec3_t v = new ccd_vec3_t();
ccdVec3Set(v, contact.pos);
ccdVec3Sub(v, t.vertices[i]);
ccdVec3Sub(v, scaledContactNormal);
if (ccdVec3Dot(edgeNormal, v) < 0) {
ccdVec3Set(v, contact.pos);
ccdVec3Sub(v, t.vertices[i]);
ccdVec3Add(v, scaledContactNormal);
if (ccdVec3Dot(edgeNormal, v) > 0) {
// Edge contact, check the edge angle now
double x = ccdVec3Dot(triangleNormal, contactNormal);
double y = ccdVec3Dot(edgeNormal, contactNormal);
double contactNormalToTriangleNormalAngle = Math.atan2(y, x);
double edgeAngle = ((DxTriMesh) geom).getEdgeAngle(contact.side2, i);
double targetAngle;
edgeContact = true;
if (edgeAngle > 0) {
// Convex edge - ensure the contact normal is within
// the allowed range formed by the two triangles' normals, if not rotate it
targetAngle = Math.min(contactNormalToTriangleNormalAngle, edgeAngle);
} else {
// Concave edge
targetAngle = 0.0;
}
double angle = targetAngle - contactNormalToTriangleNormalAngle;
if (angle != 0.0) {
ccd_quat_t q = new ccd_quat_t();
ccdQuatSetAngleAxis(q, angle, edgeAxis);
ccdQuatRotVec(contactNormal, q);
}
break;
}
}
}
// If no edge contact detected, set contact normal to triangle normal
ccd_vec3_t origContactNormal = new ccd_vec3_t();
ccdVec3Set(origContactNormal, contact.normal);
ccd_vec3_t contactNormalToUse = edgeContact ? contactNormal : triangleNormal;
contact.normal.set(ccdVec3X(contactNormalToUse), ccdVec3Y(contactNormalToUse),
ccdVec3Z(contactNormalToUse));
// Reduce the depth as the normal changed
contact.depth *= Math.max(0.0, ccdVec3Dot(origContactNormal, contactNormalToUse));
return true;
}
private int addUniqueContact(DContactGeomBuffer contacts, DContactGeom c, int contactcount, int maxcontacts) {
double minDepth = Double.MAX_VALUE;
double minDepth = c.depth;
int index = contactcount;
boolean unique = true;
for (int k = 0; k < contactcount; k++) {
DContactGeom pc = contacts.get(k);
if (Math.abs(c.pos.get0() - pc.pos.get0()) < CONTACT_POS_EPSILON
&& Math.abs(c.pos.get1() - pc.pos.get1()) < CONTACT_POS_EPSILON
&& Math.abs(c.pos.get2() - pc.pos.get2()) < CONTACT_POS_EPSILON) {
index = pc.depth + CONTACT_DEPTH_EPSILON < c.depth ? k : maxcontacts;
// Accumulate similar contacts
pc.normal.add(c.normal);
pc.depth = Math.max(pc.depth, c.depth);
unique = false;
break;
}
if (contactcount == maxcontacts && pc.depth < minDepth && pc.depth < c.depth) {
if (contactcount == maxcontacts && pc.depth < minDepth) {
minDepth = pc.depth;
index = k;
}
}
if (index < maxcontacts) {
if (unique && index < maxcontacts) {
DContactGeom contact = contacts.get(index);
contact.g1 = c.g1;
contact.g2 = c.g2;
@@ -574,7 +671,8 @@ private int addUniqueContact(DContactGeomBuffer contacts, DContactGeom c, int co
}
private int addPerturbedContacts(DGeom o1, DGeom o2, int flags, ccd_convex_t c1, ccd_triangle_t c2,
DContactGeomBuffer contacts, DVector3[] triangle, DContactGeom contact, int contactcount) {
DContactGeomBuffer contacts, DVector3[] triangle, DContactGeom contact, int contactcount,
int[] indices) {
int maxcontacts = (flags & 0xffff);
DVector3 upAxis = new DVector3(0, 1, 0);
if (Math.abs(contact.normal.dot(upAxis)) > 0.7) {
@@ -585,27 +683,36 @@ private int addPerturbedContacts(DGeom o1, DGeom o2, int flags, ccd_convex_t c1,
cross.safeNormalize();
upAxis.eqCross(cross, contact.normal);
upAxis.safeNormalize();
DQuaternion q1 = new DQuaternion();
DQuaternion q2 = new DQuaternion();
DQuaternion qr = new DQuaternion();
DVector3 p = new DVector3();
DVector3 perturbed = new DVector3();
DVector3 pos = new DVector3(contact.pos);
DContactGeomBuffer perturbedContacts = new DContactGeomBuffer(1);
DQuaternion[] q1 = new DQuaternion[]{ new DQuaternion(), new DQuaternion()};
DQuaternion[] q2 = new DQuaternion[]{ new DQuaternion(), new DQuaternion()};
double perturbationAngle = CONTACT_PERTURBATION_ANGLE;
for (int j = 0; j < 2; ++j) {
dQFromAxisAndAngle(q1[j], upAxis, perturbationAngle);
dQFromAxisAndAngle(q2[j], cross, perturbationAngle);
perturbationAngle = -perturbationAngle;
}
DQuaternion qr = new DQuaternion();
DVector3 p = new DVector3();
for (int k = 0; k < 4; k++) {
dQFromAxisAndAngle(q1, upAxis, k % 2 == 0 ? CONTACT_PERTURBATION_ANGLE : -CONTACT_PERTURBATION_ANGLE);
dQFromAxisAndAngle(q2, cross, k / 2 == 0 ? CONTACT_PERTURBATION_ANGLE : -CONTACT_PERTURBATION_ANGLE);
dQMultiply0(qr, q1, q2);
dQMultiply0(qr, q1[k % 2], q2[k / 2]);
for (int j = 0; j < 3; j++) {
p.eqDiff(triangle[j], pos);
dQuatTransform(qr, p, perturbed);
perturbed.add(pos);
c2.vertices[j].set(perturbed.get0(), perturbed.get1(), perturbed.get2());
}
setObjPosToTriangleCenter(c2);
if (ccdCollide(o1, o2, flags, perturbedContacts, c1, ccdSupportConvex, ccdCenter, c2,
ccdSupportTriangle, ccdCenter) == 1) {
perturbedContacts.get().side2 = contact.side2;
contactcount = addUniqueContact(contacts, perturbedContacts.get(), contactcount, maxcontacts);
DContactGeom perturbedContact = perturbedContacts.get();
perturbedContact.side2 = contact.side2;
if (correctContactNormal(c2, perturbedContact, triangle, o2, indices)) {
contactcount = addUniqueContact(contacts, perturbedContact, contactcount, maxcontacts);
}
}
}
return contactcount;
@@ -393,4 +393,9 @@ public DTriTriMergeCallback getTriMergeCallback() {
public void setTriMergeCallback(DTriTriMergeCallback Callback) {
dGeomTriMeshSetTriMergeCallback(Callback);
}
@Override
public float getEdgeAngle(int triangle, int edge) {
return _Data.getEdgeAngle(triangle, edge);
}
}
@@ -45,6 +45,7 @@
// int m_TriangleCount;
// int m_TriStride;
// boolean m_single;
private float[] m_Angles;
DxGimpactData()//dxTriMeshData()
{
@@ -119,7 +120,6 @@ public void build(final float[] Vertices,
//TODO remove?
//check();
}
void GetVertex(int i, DVector4 Out)
{
//TZ commented out, special treatment not required (?)
@@ -156,10 +156,8 @@ void GetTriIndices(int itriangle, int[] triindices)
//#endif // dTRIMESH_GIMPACT
@Override
void Preprocess() {
// void dxTriMeshData::Preprocess(){ // stub
// }
throw new UnsupportedOperationException(); //??????
public void preprocess() {
m_Angles = new GimpactDataPreprocessor(this).buildAngles();
}
@Override
@@ -286,11 +284,6 @@ void UpdateData() {
// null);//(const int*)NULL);
// }
//void dGeomTriMeshDataPreprocess(dTriMeshDataID g)
void dGeomTriMeshDataPreprocess()
{
Preprocess();
}
//void dGeomTriMeshDataGetBuffer(dTriMeshDataID g, unsigned char** buf, int* bufLen)
void dGeomTriMeshDataGetBuffer(Ref<Object> buf, RefInt bufLen)
@@ -405,5 +398,10 @@ public void check() {
}
System.out.println(nE);
}
public float getEdgeAngle(int triangle, int edge) {
return (float) (m_Angles != null ? m_Angles[triangle * 3 + edge] : Math.PI * 2);
}
}
@@ -81,5 +81,14 @@ public static DxTriMesh dCreateTriMesh(DxSpace space,
abstract public int FetchTriangleCount();
abstract public void FetchTransformedTriangle(int i, DVector3[] v);
/*
* Returns the following values:
* between -PI and 0 for concave edges
* 0 for flat edges
* between 0 and PI for convex edges
* > PI for boundary edges
*/
abstract public float getEdgeAngle(int triangle, int edge);
}
@@ -44,7 +44,7 @@
// };
/* Setup the UseFlags array */
abstract void Preprocess();
public abstract void preprocess();
/* For when app changes the vertices */
abstract void UpdateData();
@@ -33,7 +33,7 @@
static class dxTriMeshDisabledData extends DxTriMeshData {
@Override
void Preprocess() {
public void preprocess() {
throw new UnsupportedOperationException();
}
@@ -189,6 +189,11 @@ public void setTriMergeCallback(DTriTriMergeCallback Callback) {
}
@Override
public float getEdgeAngle(int triangle, int edge) {
return (float) (Math.PI * 2);
}
//#endif // !dTRIMESH_ENABLED
}
Oops, something went wrong.

0 comments on commit 170876f

Please sign in to comment.