/*
|
* Java port of Bullet (c) 2008 Martin Dvorak <jezek2@advel.cz>
|
*
|
* This source file is part of GIMPACT Library.
|
*
|
* For the latest info, see http://gimpact.sourceforge.net/
|
*
|
* Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
|
* email: projectileman@yahoo.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.extras.gimpact;
|
|
import com.bulletphysics.BulletGlobals;
|
import com.bulletphysics.linearmath.VectorUtil;
|
import cz.advel.stack.Stack;
|
import javax.vecmath.Vector3f;
|
import javax.vecmath.Vector4f;
|
|
/**
|
*
|
* @author jezek2
|
*/
|
class GeometryOperations {
|
|
public static final float PLANEDIREPSILON = 0.0000001f;
|
public static final float PARALELENORMALS = 0.000001f;
|
|
public static final float CLAMP(float number, float minval, float maxval) {
|
return (number < minval? minval : (number > maxval? maxval : number));
|
}
|
|
/**
|
* Calc a plane from a triangle edge an a normal.
|
*/
|
public static void edge_plane(Vector3f e1, Vector3f e2, Vector3f normal, Vector4f plane) {
|
Vector3f planenormal = Stack.alloc(Vector3f.class);
|
planenormal.sub(e2, e1);
|
planenormal.cross(planenormal, normal);
|
planenormal.normalize();
|
|
plane.set(planenormal);
|
plane.w = e2.dot(planenormal);
|
}
|
|
/**
|
* Finds the closest point(cp) to (v) on a segment (e1,e2).
|
*/
|
public static void closest_point_on_segment(Vector3f cp, Vector3f v, Vector3f e1, Vector3f e2) {
|
Vector3f n = Stack.alloc(Vector3f.class);
|
n.sub(e2, e1);
|
cp.sub(v, e1);
|
float _scalar = cp.dot(n) / n.dot(n);
|
if (_scalar < 0.0f) {
|
cp = e1;
|
}
|
else if (_scalar > 1.0f) {
|
cp = e2;
|
}
|
else {
|
cp.scaleAdd(_scalar, n, e1);
|
}
|
}
|
|
/**
|
* Line plane collision.
|
*
|
* @return -0 if the ray never intersects, -1 if the ray collides in front, -2 if the ray collides in back
|
*/
|
public static int line_plane_collision(Vector4f plane, Vector3f vDir, Vector3f vPoint, Vector3f pout, float[] tparam, float tmin, float tmax) {
|
float _dotdir = VectorUtil.dot3(vDir, plane);
|
|
if (Math.abs(_dotdir) < PLANEDIREPSILON) {
|
tparam[0] = tmax;
|
return 0;
|
}
|
|
float _dis = ClipPolygon.distance_point_plane(plane, vPoint);
|
int returnvalue = _dis < 0.0f ? 2 : 1;
|
tparam[0] = -_dis / _dotdir;
|
|
if (tparam[0] < tmin) {
|
returnvalue = 0;
|
tparam[0] = tmin;
|
}
|
else if (tparam[0] > tmax) {
|
returnvalue = 0;
|
tparam[0] = tmax;
|
}
|
pout.scaleAdd(tparam[0], vDir, vPoint);
|
return returnvalue;
|
}
|
|
/**
|
* Find closest points on segments.
|
*/
|
public static void segment_collision(Vector3f vA1, Vector3f vA2, Vector3f vB1, Vector3f vB2, Vector3f vPointA, Vector3f vPointB) {
|
Vector3f AD = Stack.alloc(Vector3f.class);
|
AD.sub(vA2, vA1);
|
|
Vector3f BD = Stack.alloc(Vector3f.class);
|
BD.sub(vB2, vB1);
|
|
Vector3f N = Stack.alloc(Vector3f.class);
|
N.cross(AD, BD);
|
float[] tp = new float[] { N.lengthSquared() };
|
|
Vector4f _M = Stack.alloc(Vector4f.class);//plane
|
|
if (tp[0] < BulletGlobals.SIMD_EPSILON)//ARE PARALELE
|
{
|
// project B over A
|
boolean invert_b_order = false;
|
_M.x = vB1.dot(AD);
|
_M.y = vB2.dot(AD);
|
|
if (_M.x > _M.y) {
|
invert_b_order = true;
|
//BT_SWAP_NUMBERS(_M[0],_M[1]);
|
_M.x = _M.x + _M.y;
|
_M.y = _M.x - _M.y;
|
_M.x = _M.x - _M.y;
|
}
|
_M.z = vA1.dot(AD);
|
_M.w = vA2.dot(AD);
|
// mid points
|
N.x = (_M.x + _M.y) * 0.5f;
|
N.y = (_M.z + _M.w) * 0.5f;
|
|
if (N.x < N.y) {
|
if (_M.y < _M.z) {
|
vPointB = invert_b_order ? vB1 : vB2;
|
vPointA = vA1;
|
}
|
else if (_M.y < _M.w) {
|
vPointB = invert_b_order ? vB1 : vB2;
|
closest_point_on_segment(vPointA, vPointB, vA1, vA2);
|
}
|
else {
|
vPointA = vA2;
|
closest_point_on_segment(vPointB, vPointA, vB1, vB2);
|
}
|
}
|
else {
|
if (_M.w < _M.x) {
|
vPointB = invert_b_order ? vB2 : vB1;
|
vPointA = vA2;
|
}
|
else if (_M.w < _M.y) {
|
vPointA = vA2;
|
closest_point_on_segment(vPointB, vPointA, vB1, vB2);
|
}
|
else {
|
vPointB = invert_b_order ? vB1 : vB2;
|
closest_point_on_segment(vPointA, vPointB, vA1, vA2);
|
}
|
}
|
return;
|
}
|
|
N.cross(N, BD);
|
_M.set(N.x, N.y, N.z, vB1.dot(N));
|
|
// get point A as the plane collision point
|
line_plane_collision(_M, AD, vA1, vPointA, tp, 0f, 1f);
|
|
/*Closest point on segment*/
|
vPointB.sub(vPointA, vB1);
|
tp[0] = vPointB.dot(BD);
|
tp[0] /= BD.dot(BD);
|
tp[0] = CLAMP(tp[0], 0.0f, 1.0f);
|
|
vPointB.scaleAdd(tp[0], BD, vB1);
|
}
|
|
}
|
