com.bulletphysics.collision.narrowphase.PersistentManifold Maven / Gradle / Ivy
Show all versions of jbullet Show documentation
/*
* Java port of Bullet (c) 2008 Martin Dvorak
*
* Bullet Continuous Collision Detection and Physics Library
* Copyright (c) 2003-2008 Erwin Coumans http://www.bulletphysics.com/
*
* This software is provided 'as-is', without any express or implied warranty.
* In no event will the authors be held liable for any damages arising from
* the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
package com.bulletphysics.collision.narrowphase;
import com.bulletphysics.BulletGlobals;
import com.bulletphysics.linearmath.Transform;
import com.bulletphysics.linearmath.VectorUtil;
import cz.advel.stack.Stack;
import javax.vecmath.Vector3f;
import javax.vecmath.Vector4f;
/**
* PersistentManifold is a contact point cache, it stays persistent as long as objects
* are overlapping in the broadphase. Those contact points are created by the collision
* narrow phase.
*
* The cache can be empty, or hold 1, 2, 3 or 4 points. Some collision algorithms (GJK)
* might only add one point at a time, updates/refreshes old contact points, and throw
* them away if necessary (distance becomes too large).
*
* Reduces the cache to 4 points, when more then 4 points are added, using following rules:
* the contact point with deepest penetration is always kept, and it tries to maximize the
* area covered by the points.
*
* Note that some pairs of objects might have more then one contact manifold.
*
* @author jezek2
*/
public class PersistentManifold {
//protected final BulletStack stack = BulletStack.get();
public static final int MANIFOLD_CACHE_SIZE = 4;
private final ManifoldPoint[] pointCache = new ManifoldPoint[MANIFOLD_CACHE_SIZE];
/// this two body pointers can point to the physics rigidbody class.
/// void* will allow any rigidbody class
private Object body0;
private Object body1;
private int cachedPoints;
public int index1a;
{
for (int i=0; i 1) {
// throw new InternalError();
// }
// }
// }
// assert (occurance <= 0);
//#endif //DEBUG_PERSISTENCY
if (pt.userPersistentData != null && BulletGlobals.getContactDestroyedCallback() != null) {
BulletGlobals.getContactDestroyedCallback().contactDestroyed(pt.userPersistentData);
pt.userPersistentData = null;
}
//#ifdef DEBUG_PERSISTENCY
// DebugPersistency();
//#endif
}
}
public int getNumContacts() {
return cachedPoints;
}
public ManifoldPoint getContactPoint(int index) {
return pointCache[index];
}
// todo: get this margin from the current physics / collision environment
public float getContactBreakingThreshold() {
return BulletGlobals.getContactBreakingThreshold();
}
public int getCacheEntry(ManifoldPoint newPoint) {
float shortestDist = getContactBreakingThreshold() * getContactBreakingThreshold();
int size = getNumContacts();
int nearestPoint = -1;
Vector3f diffA = Stack.alloc(Vector3f.class);
for (int i = 0; i < size; i++) {
ManifoldPoint mp = pointCache[i];
diffA.sub(mp.localPointA, newPoint.localPointA);
float distToManiPoint = diffA.dot(diffA);
if (distToManiPoint < shortestDist) {
shortestDist = distToManiPoint;
nearestPoint = i;
}
}
return nearestPoint;
}
public int addManifoldPoint(ManifoldPoint newPoint) {
assert (validContactDistance(newPoint));
int insertIndex = getNumContacts();
if (insertIndex == MANIFOLD_CACHE_SIZE) {
//#if MANIFOLD_CACHE_SIZE >= 4
if (MANIFOLD_CACHE_SIZE >= 4) {
//sort cache so best points come first, based on area
insertIndex = sortCachedPoints(newPoint);
}
else {
//#else
insertIndex = 0;
}
//#endif
clearUserCache(pointCache[insertIndex]);
}
else {
cachedPoints++;
}
assert (pointCache[insertIndex].userPersistentData == null);
pointCache[insertIndex].set(newPoint);
return insertIndex;
}
public void removeContactPoint(int index) {
clearUserCache(pointCache[index]);
int lastUsedIndex = getNumContacts() - 1;
// m_pointCache[index] = m_pointCache[lastUsedIndex];
if (index != lastUsedIndex) {
// TODO: possible bug
pointCache[index].set(pointCache[lastUsedIndex]);
//get rid of duplicated userPersistentData pointer
pointCache[lastUsedIndex].userPersistentData = null;
pointCache[lastUsedIndex].appliedImpulse = 0f;
pointCache[lastUsedIndex].lateralFrictionInitialized = false;
pointCache[lastUsedIndex].appliedImpulseLateral1 = 0f;
pointCache[lastUsedIndex].appliedImpulseLateral2 = 0f;
pointCache[lastUsedIndex].lifeTime = 0;
}
assert (pointCache[lastUsedIndex].userPersistentData == null);
cachedPoints--;
}
public void replaceContactPoint(ManifoldPoint newPoint, int insertIndex) {
assert (validContactDistance(newPoint));
//#define MAINTAIN_PERSISTENCY 1
//#ifdef MAINTAIN_PERSISTENCY
int lifeTime = pointCache[insertIndex].getLifeTime();
float appliedImpulse = pointCache[insertIndex].appliedImpulse;
float appliedLateralImpulse1 = pointCache[insertIndex].appliedImpulseLateral1;
float appliedLateralImpulse2 = pointCache[insertIndex].appliedImpulseLateral2;
assert (lifeTime >= 0);
Object cache = pointCache[insertIndex].userPersistentData;
pointCache[insertIndex].set(newPoint);
pointCache[insertIndex].userPersistentData = cache;
pointCache[insertIndex].appliedImpulse = appliedImpulse;
pointCache[insertIndex].appliedImpulseLateral1 = appliedLateralImpulse1;
pointCache[insertIndex].appliedImpulseLateral2 = appliedLateralImpulse2;
pointCache[insertIndex].lifeTime = lifeTime;
//#else
// clearUserCache(m_pointCache[insertIndex]);
// m_pointCache[insertIndex] = newPoint;
//#endif
}
private boolean validContactDistance(ManifoldPoint pt) {
return pt.distance1 <= getContactBreakingThreshold();
}
/// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin
public void refreshContactPoints(Transform trA, Transform trB) {
Vector3f tmp = Stack.alloc(Vector3f.class);
int i;
//#ifdef DEBUG_PERSISTENCY
// printf("refreshContactPoints posA = (%f,%f,%f) posB = (%f,%f,%f)\n",
// trA.getOrigin().getX(),
// trA.getOrigin().getY(),
// trA.getOrigin().getZ(),
// trB.getOrigin().getX(),
// trB.getOrigin().getY(),
// trB.getOrigin().getZ());
//#endif //DEBUG_PERSISTENCY
// first refresh worldspace positions and distance
for (i = getNumContacts() - 1; i >= 0; i--) {
ManifoldPoint manifoldPoint = pointCache[i];
manifoldPoint.positionWorldOnA.set(manifoldPoint.localPointA);
trA.transform(manifoldPoint.positionWorldOnA);
manifoldPoint.positionWorldOnB.set(manifoldPoint.localPointB);
trB.transform(manifoldPoint.positionWorldOnB);
tmp.set(manifoldPoint.positionWorldOnA);
tmp.sub(manifoldPoint.positionWorldOnB);
manifoldPoint.distance1 = tmp.dot(manifoldPoint.normalWorldOnB);
manifoldPoint.lifeTime++;
}
// then
float distance2d;
Vector3f projectedDifference = Stack.alloc(Vector3f.class), projectedPoint = Stack.alloc(Vector3f.class);
for (i = getNumContacts() - 1; i >= 0; i--) {
ManifoldPoint manifoldPoint = pointCache[i];
// contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
if (!validContactDistance(manifoldPoint)) {
removeContactPoint(i);
}
else {
// contact also becomes invalid when relative movement orthogonal to normal exceeds margin
tmp.scale(manifoldPoint.distance1, manifoldPoint.normalWorldOnB);
projectedPoint.sub(manifoldPoint.positionWorldOnA, tmp);
projectedDifference.sub(manifoldPoint.positionWorldOnB, projectedPoint);
distance2d = projectedDifference.dot(projectedDifference);
if (distance2d > getContactBreakingThreshold() * getContactBreakingThreshold()) {
removeContactPoint(i);
}
else {
// contact point processed callback
if (BulletGlobals.getContactProcessedCallback() != null) {
BulletGlobals.getContactProcessedCallback().contactProcessed(manifoldPoint, body0, body1);
}
}
}
}
//#ifdef DEBUG_PERSISTENCY
// DebugPersistency();
//#endif //
}
public void clearManifold() {
int i;
for (i = 0; i < cachedPoints; i++) {
clearUserCache(pointCache[i]);
}
cachedPoints = 0;
}
}