Skip to content

Instantly share code, notes, and snippets.

@bgilbert6
Created October 15, 2016 17:00
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
Star You must be signed in to star a gist
Save bgilbert6/cb8db8251364b210649020344987d482 to your computer and use it in GitHub Desktop.
Simple Ellipsoid Collideable
/*
* To change this template, choose Tools | Templates
* and open the template in the editor.
*/
package mygame;
import com.jme3.math.FastMath;
import com.jme3.math.Plane;
import com.jme3.math.Ray;
import com.jme3.math.Triangle;
import com.jme3.math.Vector3f;
import java.util.List;
/**
*
* @author bgilb
*/
public class CollideableEllipsoid {
Vector3f eSize;
private float epsilon = 0.00001f;
private float collisionBuffer = 0.00002f;
private int collisionDepth = 0;
private int maxCollisionDepth = 5;
/*
* @eSize Ellipsoid size
*/
public CollideableEllipsoid(Vector3f eSize) {
this.eSize = eSize;
}
//At what velocity do we not care to check for a collision, to prevent vibrations.
public void setEpsilon(float f) {
epsilon = f;
}
//How close do we allow the ellipsoid to move towards the triangle. We don't want to let the player move right
//on the triangle because then they would end up intersected.
public void setCollisionBuffer(float f) {
collisionBuffer = f;
}
public void setSize(Vector3f eSize) {
this.eSize = eSize;
}
public Vector3f getSize() {
return eSize;
}
public void setMaxCollisionDepth(int depth) {
maxCollisionDepth = depth;
}
/*
* @position The current ellipsoid position
* @velocity Dir+speed they want to move
* @triangles Up to you to supply which triangles to check for a collision. Could be the entire scene
* @return The new position after handling collisions.
* or use OcTree, etc.
*/
public Vector3f collideWithWorld(Vector3f position, Vector3f velocity, List<Triangle> triangles) {
//transform to e space
Vector3f ePosition = position.divide(eSize);
Vector3f eVelocity = velocity.divide(eSize);
collisionDepth = 0;
ePosition = collideWithWorldESpace(ePosition, eVelocity, triangles);
//transform out of espace
return ePosition.mult(eSize);
}
private Vector3f collideWithWorldESpace(Vector3f position, Vector3f velocity, List<Triangle> triangles) {
if (velocity.length() < epsilon) {
return position;
}
Vector3f destPos = position.add(velocity);
if (triangles.isEmpty()) {
return position.add(velocity);
}
if(collisionDepth >= 5) {
return position;
}
collisionDepth++;
boolean foundCollision = false;
float nearestDistance = -1f;
Vector3f nearestSphereIntersectPoint = null;
Vector3f nearestPolygonIntersectPoint = null;
for (Triangle t : triangles) {
Triangle newT = new Triangle();
//transform to eSpace
newT.set1(t.get1().divide(eSize));
newT.set2(t.get2().divide(eSize));
newT.set3(t.get3().divide(eSize));
newT.calculateCenter();
newT.calculateNormal();
Plane plane = new Plane();
plane.setPlanePoints(newT);
Vector3f sphereIntersectPoint = position.subtract(newT.getNormal().normalize());
Vector3f planeIntersectPoint = new Vector3f();
if(plane.whichSide(sphereIntersectPoint) == Plane.Side.Negative) {
Ray ray = new Ray(sphereIntersectPoint, newT.getNormal().normalize());
ray.intersectsWherePlane(plane, planeIntersectPoint);
} else {
Ray ray = new Ray(sphereIntersectPoint, velocity.normalize());
ray.intersectsWherePlane(plane, planeIntersectPoint);
}
//assume its plane point
Vector3f polygonIntersectPoint = planeIntersectPoint.clone();
if(!isPointInTriangle(polygonIntersectPoint, newT)) {
polygonIntersectPoint = closestPointOnTriangle(newT, polygonIntersectPoint);
//maybe it doesn't even hit the sphere?
float distToSphere = intersectRaySphere(polygonIntersectPoint, velocity.negate().normalize(), position, 1.0f);
if(distToSphere > 0) {
sphereIntersectPoint.x = polygonIntersectPoint.x + distToSphere * velocity.negate().normalize().x;
sphereIntersectPoint.y = polygonIntersectPoint.y + distToSphere * velocity.negate().normalize().y;
sphereIntersectPoint.z = polygonIntersectPoint.z + distToSphere * velocity.negate().normalize().z;
} else {
//no collision
sphereIntersectPoint = null;
}
}
if(sphereIntersectPoint != null) {
float distToSphere = sphereIntersectPoint.distance(polygonIntersectPoint);
if((distToSphere > 0) && (distToSphere <= velocity.length())) {
if(foundCollision == false || distToSphere < nearestDistance) {
nearestDistance = distToSphere;
nearestSphereIntersectPoint = sphereIntersectPoint.clone();
nearestPolygonIntersectPoint = polygonIntersectPoint.clone();
foundCollision = true;
}
}
}
}
if(foundCollision) {
Vector3f moveVel;
if (nearestDistance >= collisionBuffer) {
moveVel = velocity.clone();
setLength(moveVel, nearestDistance - collisionBuffer);
} else {
moveVel = velocity.negate();
setLength(moveVel, Math.abs(nearestDistance - collisionBuffer));
}
Vector3f newPosition = position.add(moveVel);
Vector3f slidePlaneNormal = newPosition.subtract(nearestPolygonIntersectPoint).normalize();
Vector3f slideVector = velocity.subtract(slidePlaneNormal.mult(slidePlaneNormal.dot(velocity)));//.mult(leftOver);
//slideVector.addLocal(moveVel);
//setLength(slideVector, velocity.length());
Vector3f newVelocity = slideVector;
System.out.println("Depth: " + collisionDepth);
System.out.println("New Velocity" + newVelocity);
System.out.println("Position" + position);
return collideWithWorldESpace(newPosition, newVelocity, triangles);
}
return position.add(velocity);
}
private Vector3f closestPointOnTriangle(Triangle t, Vector3f p) {
Vector3f rab = closestPointOnLine(t.get1(), t.get2(), p);
Vector3f rbc = closestPointOnLine(t.get2(), t.get3(), p);
Vector3f rca = closestPointOnLine(t.get3(), t.get1(), p);
float dAB = p.subtract(rab).length();
float dBC = p.subtract(rbc).length();
float dCA = p.subtract(rca).length();
float min = dAB;
Vector3f result = rab;
if (dBC < min) {
min = dBC;
result = rbc;
}
if (dCA < min) {
result = rca;
}
return result.clone();
}
private Vector3f closestPointOnLine(Vector3f a, Vector3f b, Vector3f p) {
// Determine t (the length of the vector from ‘a’ to ‘p’)
Vector3f c = p.subtract(a);
Vector3f V = b.subtract(a);
float d = V.length();
V.normalizeLocal();
float t = V.dot(c);
// Check to see if ‘t’ is beyond the extents of the line segment
if (t < 0.0f) {
return (a);
}
if (t > d) {
return (b);
}
// Return the point between ‘a’ and ‘b’
//set length of V to t. V is normalized so this is easy
V.x = V.x * t;
V.y = V.y * t;
V.z = V.z * t;
return a.add(V);
}
private float intersectRaySphere(Vector3f rO, Vector3f rV, Vector3f sO, float sR) {
Vector3f Q = sO.subtract(rO);
float c = Q.length();
float v = Q.dot(rV);
float d = sR * sR - (c * c - v * v);
// If there was no intersection, return -1
if (d < 0.0) {
return (-1.0f);
}
// Return the distance to the [first] intersecting point
return (v - FastMath.sqrt(d));
}
private static void setLength(Vector3f v, float l) {
float len = FastMath.sqrt(v.x * v.x + v.y * v.y + v.z * v.z);
v.x *= l / len;
v.y *= l / len;
v.z *= l / len;
}
private boolean isPointInSphere(Vector3f point, Vector3f sO, float sR) {
float d = point.subtract(sO).length();
if (d <= sR) {
return true;
}
return false;
}
private boolean isPointInTriangle(Vector3f point, Triangle triangle) {
Vector3f e10 = triangle.get2().subtract(triangle.get1());
Vector3f e20 = triangle.get3().subtract(triangle.get1());
float a = e10.dot(e10);
float b = e10.dot(e20);
float c = e20.dot(e20);
float ac_bb = (a * c) - (b * b);
//VECTOR vp(point.x-pa.x, point.y-pa.y, point.z-pa.z);
Vector3f pa = triangle.get1();
Vector3f vp = new Vector3f(point.x - pa.x, point.y - pa.y, point.z - pa.z);
float d = vp.dot(e10);
float e = vp.dot(e20);
float x = (d * c) - (e * b);
float y = (e * a) - (d * b);
float z = x + y - ac_bb;
return z < 0 && x >= 0 && y >= 0;
}
}
//Another way to get colliders using 2 bounding boxes
public ArrayList<Triangle> getPossibleCollisions(Vector3f position, Vector3f velocity) {
//transform back into regular space
Vector3f destPos = position.add(velocity);
playerBounds.setCenter(destPos);
ArrayList<Triangle> triangles = new ArrayList<Triangle>();
CollisionResults results = new CollisionResults();
mapNode.collideWith(playerBounds, results);
for (CollisionResult result : results) {
Triangle triangle = new Triangle();
result.getTriangle(triangle);
Transform transform = result.getGeometry().getWorldTransform();
triangle.set1(transform.transformVector(triangle.get1(), null));
triangle.set2(transform.transformVector(triangle.get2(), null));
triangle.set3(transform.transformVector(triangle.get3(), null));
triangle.calculateNormal();
triangle.calculateCenter();
triangles.add(triangle);
}
//check non dest position
results = new CollisionResults();
playerBounds.setCenter(position);
mapNode.collideWith(playerBounds, results);
for (CollisionResult result : results) {
Triangle triangle = new Triangle();
result.getTriangle(triangle);
//transform to world space
Transform transform = result.getGeometry().getWorldTransform();
triangle.set1(transform.transformVector(triangle.get1(), null));
triangle.set2(transform.transformVector(triangle.get2(), null));
triangle.set3(transform.transformVector(triangle.get3(), null));
triangle.calculateNormal();
triangle.calculateCenter();
triangles.add(triangle);
}
return triangles;
}
package mygame;
import com.jme3.app.SimpleApplication;
import com.jme3.bounding.BoundingBox;
import com.jme3.collision.CollisionResult;
import com.jme3.collision.CollisionResults;
import com.jme3.input.KeyInput;
import com.jme3.input.controls.ActionListener;
import com.jme3.input.controls.KeyTrigger;
import com.jme3.light.AmbientLight;
import com.jme3.light.DirectionalLight;
import com.jme3.material.Material;
import com.jme3.math.ColorRGBA;
import com.jme3.math.FastMath;
import com.jme3.math.Transform;
import com.jme3.math.Triangle;
import com.jme3.math.Vector3f;
import com.jme3.scene.Geometry;
import com.jme3.scene.Node;
import com.jme3.scene.shape.Box;
import com.jme3.scene.shape.Sphere;
import com.jme3.system.AppSettings;
import java.util.ArrayList;
/**
* test
*
* @author normenhansen
*/
public class Main extends SimpleApplication implements ActionListener {
CollideableEllipsoid playerCollideable =
new CollideableEllipsoid(new Vector3f(0.5f, 1f, 0.5f));
Geometry player;
boolean up, down, left, right, forwards, backwards;
Node walls = new Node();
public static void main(String[] args) {
Main app = new Main();
AppSettings newSetting = new AppSettings(true);
newSetting.setFrameRate(60);
newSetting.setWidth(800);
newSetting.setHeight(600);
//newSetting.setVSync(true);
app.setSettings(newSetting);
app.setShowSettings(false);
app.setDisplayStatView(false);
app.setPauseOnLostFocus(false);
app.start();
}
@Override
public void simpleInitApp() {
flyCam.setMoveSpeed(50);
player = createPlayer();
player.setLocalTranslation(0, 1.1f, 0);
rootNode.attachChild(player);
DirectionalLight sun = new DirectionalLight();
sun.setDirection(new Vector3f(-0.15f, -0.7f, -0.15f));
rootNode.addLight(sun);
AmbientLight al = new AmbientLight();
al.setColor(ColorRGBA.White.mult(0.25f));
rootNode.addLight(al);
setKeys();
buildWalls();
rootNode.attachChild(walls);
}
public void buildWalls() {
buildWall(new Vector3f(0,-0.1f,0), new Vector3f(10, 0.1f, 10));
buildWall(new Vector3f(5,-0.1f,4), new Vector3f(3, 5f, 3));
}
public void buildWall(Vector3f pos, Vector3f size) {
Box b = new Box(size.x, size.y, size.z); // create cube shape
Geometry geom = new Geometry("Box", b); // create cube geometry from the shape
Material mat = new Material(assetManager, // Create new material and...
"Common/MatDefs/Light/Lighting.j3md"); // ... specify .j3md file to use (illuminated).
mat.setBoolean("UseMaterialColors",true); // Set some parameters, e.g. blue.
mat.setColor("Ambient", ColorRGBA.Blue); // ... color of this object
mat.setColor("Diffuse", ColorRGBA.Blue); // ... color of light being reflected
geom.setMaterial(mat); // set the cube's material
geom.setLocalTranslation(pos);
walls.attachChild(geom);
}
public Geometry createPlayer() {
Sphere b1 = new Sphere(128, 128, 1);
Geometry geom = new Geometry("Box", b1);
Material mat = new Material(assetManager, // Create new material and...
"Common/MatDefs/Light/Lighting.j3md"); // ... specify .j3md file to use (illuminated).
mat.setBoolean("UseMaterialColors",true); // Set some parameters, e.g. blue.
mat.setColor("Ambient", ColorRGBA.Green); // ... color of this object
mat.setColor("Diffuse", ColorRGBA.Green); // ... color of light being reflected
geom.setMaterial(mat);
//geom.setQueueBucket(Bucket.Transparent);
geom.scale(playerCollideable.getSize().x, playerCollideable.getSize().y, playerCollideable.getSize().z);
return geom;
}
@Override
public void simpleUpdate(float tpf) {
Vector3f velocity = new Vector3f();
if(up) {
velocity.addLocal(0, 0.03f, 0);
}
if(down) {
velocity.addLocal(0, -0.03f, 0);
}
if(left) {
velocity.addLocal(-0.03f, 0, 0);
}
if(right) {
velocity.addLocal(0.03f, 0, 0);
}
if(forwards) {
velocity.addLocal(0, 0, -0.03f);
}
if(backwards) {
velocity.addLocal(0, 0, 0.03f);
}
Vector3f newPosition = playerCollideable
.collideWithWorld(player.getLocalTranslation(), velocity, getColliders());
player.setLocalTranslation(newPosition);
}
public void setKeys() {
inputManager.addMapping("forwards", new KeyTrigger(KeyInput.KEY_I));
inputManager.addMapping("backwards", new KeyTrigger(KeyInput.KEY_K));
inputManager.addMapping("left", new KeyTrigger(KeyInput.KEY_J));
inputManager.addMapping("right", new KeyTrigger(KeyInput.KEY_L));
inputManager.addMapping("up", new KeyTrigger(KeyInput.KEY_Y));
inputManager.addMapping("down", new KeyTrigger(KeyInput.KEY_H));
inputManager.addListener(this, new String[]{"forwards", "backwards", "left", "right", "up", "down"});
}
public void onAction(String name, boolean isPressed, float tpf) {
if (name.equals("forwards")) {
forwards = isPressed;
}
if (name.equals("backwards")) {
backwards = isPressed;
}
if (name.equals("left")){
left = isPressed;
}
if (name.equals("right")) {
right = isPressed;
}
if (name.equals("up")) {
up = isPressed;
}
if (name.equals("down")) {
down = isPressed;
}
}
public ArrayList<Triangle> getColliders() {
ArrayList r = new ArrayList();
BoundingBox box = new BoundingBox(Vector3f.ZERO, 500, 500, 500);
CollisionResults results = new CollisionResults();
walls.collideWith(box, results);
for(CollisionResult result : results) {
Triangle triangle = new Triangle();
result.getTriangle(triangle);
Transform transform = result.getGeometry().getWorldTransform();
triangle.set1(transform.transformVector(triangle.get1(), null));
triangle.set2(transform.transformVector(triangle.get2(), null));
triangle.set3(transform.transformVector(triangle.get3(), null));
//triangle.
triangle.calculateNormal();
triangle.calculateCenter();
r.add(triangle);
}
return r;
}
public float intersectEllipsoid(Vector3f rOrigin, Vector3f rNormal, Vector3f eOrigin, Vector3f eRadius) {
Vector3f ro = rOrigin.subtract(eOrigin);
rNormal.normalize();
float a = ((rNormal.x * rNormal.x) / (eRadius.x * eRadius.x))
+ ((rNormal.y * rNormal.y) / (eRadius.y * eRadius.y))
+ ((rNormal.z * rNormal.z) / (eRadius.z * eRadius.z));
float b = ((2 * ro.x * rNormal.x) / (eRadius.x * eRadius.x))
+ ((2 * ro.y * rNormal.y) / (eRadius.y * eRadius.y))
+ ((2 * ro.z * rNormal.z) / (eRadius.z * eRadius.z));
float c = ((ro.x * ro.x) / (eRadius.x * eRadius.x))
+ ((ro.y * ro.y) / (eRadius.y * eRadius.y))
+ ((ro.z * ro.z) / (eRadius.z * eRadius.z))
- 1;
float d = ((b * b) - (4 * a * c));
if (d < 0) {
return -1;
} else {
d = FastMath.sqrt(d);
}
float hit = (-b + d) / (2 * a);
float hitsecond = (-b - d) / (2 * a);
//DEBUG
//std::cout << "Hit1: " << hit << " Hit2: " << hitsecond << std::endl;
if (hit < hitsecond) {
return hit;
} else {
return hitsecond;
}
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment