public Vector3f calculateForce(
Vector3f location,
Vector3f velocity,
float collisionRadius,
float speed,
float turnSpeed,
float tpf,
List<Obstacle> obstacles) {
float cautionRange = speed * 1.5f / turnSpeed;
Line line = new Line(location, velocity.normalize());
Plane plane = new Plane(velocity, 1);
Vector3f closest = Vector3f.ZERO;
float shortestDistance = -1;
for (Obstacle obs : obstacles) {
Vector3f target = obs.getLocation();
Vector3f loc = target.subtract(location);
// If the obstacle isn't ahead of him, just ignore it
if (plane.whichSide(loc) != Side.Positive) {
continue;
}
// Check if the target is inside the check range
if (location.distance(target) <= collisionRadius + cautionRange + obs.getRadius()) {
// Check if the target will collide with the source
if (obs.distance(line) < collisionRadius) {
float newDistance = obs.distance(location);
// Store the closest target
if (shortestDistance == -1 || newDistance < shortestDistance)
shortestDistance = newDistance;
closest = target;
}
}
}
// If any target was found
if (shortestDistance != -1) {
// Find in wich side the target is
// To do that, we do a signed distance by
// subtracing the location from the target
// and the dot product between the line's normal
float dot = closest.subtract(location).dot(line.getDirection().cross(Vector3f.UNIT_Y));
if (dot <= 0) return velocity.cross(Vector3f.UNIT_Y);
else return velocity.cross(Vector3f.UNIT_Y).negate();
}
// No target found, just return a zero value
return Vector3f.ZERO;
}
public static TriangleData processTriangle(int[] index, Vector3f[] v, Vector2f[] t) {
Vector3f edge1 = new Vector3f();
Vector3f edge2 = new Vector3f();
Vector2f edge1uv = new Vector2f();
Vector2f edge2uv = new Vector2f();
Vector3f tangent = new Vector3f();
Vector3f binormal = new Vector3f();
Vector3f normal = new Vector3f();
t[1].subtract(t[0], edge1uv);
t[2].subtract(t[0], edge2uv);
float det = edge1uv.x * edge2uv.y - edge1uv.y * edge2uv.x;
boolean normalize = false;
if (Math.abs(det) < ZERO_TOLERANCE) {
// log.log(Level.WARNING, "Colinear uv coordinates for triangle " + "[{0}, {1}, {2}]; tex0 =
// [{3}, {4}], " + "tex1 = [{5}, {6}], tex2 = [{7}, {8}]",
// new Object[] { index[0], index[1], index[2], t[0].x, t[0].y, t[1].x, t[1].y, t[2].x,
// t[2].y });
det = 1;
normalize = true;
}
v[1].subtract(v[0], edge1);
v[2].subtract(v[0], edge2);
tangent.set(edge1);
tangent.normalizeLocal();
binormal.set(edge2);
binormal.normalizeLocal();
if (Math.abs(Math.abs(tangent.dot(binormal)) - 1) < ZERO_TOLERANCE) {
// log.log(Level.WARNING, "Vertices are on the same line " + "for triangle [{0}, {1},
// {2}].", new Object[] { index[0], index[1], index[2] });
}
float factor = 1 / det;
tangent.x = (edge2uv.y * edge1.x - edge1uv.y * edge2.x) * factor;
tangent.y = (edge2uv.y * edge1.y - edge1uv.y * edge2.y) * factor;
tangent.z = (edge2uv.y * edge1.z - edge1uv.y * edge2.z) * factor;
if (normalize) {
tangent.normalizeLocal();
}
binormal.x = (edge1uv.x * edge2.x - edge2uv.x * edge1.x) * factor;
binormal.y = (edge1uv.x * edge2.y - edge2uv.x * edge1.y) * factor;
binormal.z = (edge1uv.x * edge2.z - edge2uv.x * edge1.z) * factor;
if (normalize) {
binormal.normalizeLocal();
}
tangent.cross(binormal, normal);
normal.normalizeLocal();
return new TriangleData(tangent, binormal, normal);
}
/**
* Rebuilds the cylinder based on a new set of parameters.
*
* @param axisSamples the number of samples along the axis.
* @param radialSamples the number of samples around the radial.
* @param radius the radius of the bottom of the cylinder.
* @param radius2 the radius of the top of the cylinder.
* @param height the cylinder's height.
* @param closed should the cylinder have top and bottom surfaces.
* @param inverted is the cylinder is meant to be viewed from the inside.
*/
public void updateGeometry(
int axisSamples,
int radialSamples,
float radius,
float radius2,
float height,
boolean closed,
boolean inverted) {
this.axisSamples = axisSamples + (closed ? 2 : 0);
this.radialSamples = radialSamples;
this.radius = radius;
this.radius2 = radius2;
this.height = height;
this.closed = closed;
this.inverted = inverted;
// VertexBuffer pvb = getBuffer(Type.Position);
// VertexBuffer nvb = getBuffer(Type.Normal);
// VertexBuffer tvb = getBuffer(Type.TexCoord);
// Vertices
int vertCount = axisSamples * (radialSamples + 1) + (closed ? 2 : 0);
setBuffer(Type.Position, 3, createVector3Buffer(getFloatBuffer(Type.Position), vertCount));
// Normals
setBuffer(Type.Normal, 3, createVector3Buffer(getFloatBuffer(Type.Normal), vertCount));
// Texture co-ordinates
setBuffer(Type.TexCoord, 2, createVector2Buffer(vertCount));
int triCount = ((closed ? 2 : 0) + 2 * (axisSamples - 1)) * radialSamples;
setBuffer(Type.Index, 3, createShortBuffer(getShortBuffer(Type.Index), 3 * triCount));
// generate geometry
float inverseRadial = 1.0f / radialSamples;
float inverseAxisLess = 1.0f / (closed ? axisSamples - 3 : axisSamples - 1);
float inverseAxisLessTexture = 1.0f / (axisSamples - 1);
float halfHeight = 0.5f * height;
// Generate points on the unit circle to be used in computing the mesh
// points on a cylinder slice.
float[] sin = new float[radialSamples + 1];
float[] cos = new float[radialSamples + 1];
for (int radialCount = 0; radialCount < radialSamples; radialCount++) {
float angle = FastMath.TWO_PI * inverseRadial * radialCount;
cos[radialCount] = FastMath.cos(angle);
sin[radialCount] = FastMath.sin(angle);
}
sin[radialSamples] = sin[0];
cos[radialSamples] = cos[0];
// calculate normals
Vector3f[] vNormals = null;
Vector3f vNormal = Vector3f.UNIT_Z;
if ((height != 0.0f) && (radius != radius2)) {
vNormals = new Vector3f[radialSamples];
Vector3f vHeight = Vector3f.UNIT_Z.mult(height);
Vector3f vRadial = new Vector3f();
for (int radialCount = 0; radialCount < radialSamples; radialCount++) {
vRadial.set(cos[radialCount], sin[radialCount], 0.0f);
Vector3f vRadius = vRadial.mult(radius);
Vector3f vRadius2 = vRadial.mult(radius2);
Vector3f vMantle = vHeight.subtract(vRadius2.subtract(vRadius));
Vector3f vTangent = vRadial.cross(Vector3f.UNIT_Z);
vNormals[radialCount] = vMantle.cross(vTangent).normalize();
}
}
FloatBuffer nb = getFloatBuffer(Type.Normal);
FloatBuffer pb = getFloatBuffer(Type.Position);
FloatBuffer tb = getFloatBuffer(Type.TexCoord);
// generate the cylinder itself
Vector3f tempNormal = new Vector3f();
for (int axisCount = 0, i = 0; axisCount < axisSamples; axisCount++, i++) {
float axisFraction;
float axisFractionTexture;
int topBottom = 0;
if (!closed) {
axisFraction = axisCount * inverseAxisLess; // in [0,1]
axisFractionTexture = axisFraction;
} else {
if (axisCount == 0) {
topBottom = -1; // bottom
axisFraction = 0;
axisFractionTexture = inverseAxisLessTexture;
} else if (axisCount == axisSamples - 1) {
topBottom = 1; // top
axisFraction = 1;
axisFractionTexture = 1 - inverseAxisLessTexture;
} else {
axisFraction = (axisCount - 1) * inverseAxisLess;
axisFractionTexture = axisCount * inverseAxisLessTexture;
}
}
// compute center of slice
float z = -halfHeight + height * axisFraction;
Vector3f sliceCenter = new Vector3f(0, 0, z);
// compute slice vertices with duplication at end point
int save = i;
for (int radialCount = 0; radialCount < radialSamples; radialCount++, i++) {
float radialFraction = radialCount * inverseRadial; // in [0,1)
tempNormal.set(cos[radialCount], sin[radialCount], 0.0f);
if (vNormals != null) {
vNormal = vNormals[radialCount];
} else if (radius == radius2) {
vNormal = tempNormal;
}
if (topBottom == 0) {
if (!inverted) nb.put(vNormal.x).put(vNormal.y).put(vNormal.z);
else nb.put(-vNormal.x).put(-vNormal.y).put(-vNormal.z);
} else {
nb.put(0).put(0).put(topBottom * (inverted ? -1 : 1));
}
tempNormal.multLocal((radius - radius2) * axisFraction + radius2).addLocal(sliceCenter);
pb.put(tempNormal.x).put(tempNormal.y).put(tempNormal.z);
tb.put((inverted ? 1 - radialFraction : radialFraction)).put(axisFractionTexture);
}
BufferUtils.copyInternalVector3(pb, save, i);
BufferUtils.copyInternalVector3(nb, save, i);
tb.put((inverted ? 0.0f : 1.0f)).put(axisFractionTexture);
}
if (closed) {
pb.put(0).put(0).put(-halfHeight); // bottom center
nb.put(0).put(0).put(-1 * (inverted ? -1 : 1));
tb.put(0.5f).put(0);
pb.put(0).put(0).put(halfHeight); // top center
nb.put(0).put(0).put(1 * (inverted ? -1 : 1));
tb.put(0.5f).put(1);
}
IndexBuffer ib = getIndexBuffer();
int index = 0;
// Connectivity
for (int axisCount = 0, axisStart = 0; axisCount < axisSamples - 1; axisCount++) {
int i0 = axisStart;
int i1 = i0 + 1;
axisStart += radialSamples + 1;
int i2 = axisStart;
int i3 = i2 + 1;
for (int i = 0; i < radialSamples; i++) {
if (closed && axisCount == 0) {
if (!inverted) {
ib.put(index++, i0++);
ib.put(index++, vertCount - 2);
ib.put(index++, i1++);
} else {
ib.put(index++, i0++);
ib.put(index++, i1++);
ib.put(index++, vertCount - 2);
}
} else if (closed && axisCount == axisSamples - 2) {
ib.put(index++, i2++);
ib.put(index++, inverted ? vertCount - 1 : i3++);
ib.put(index++, inverted ? i3++ : vertCount - 1);
} else {
ib.put(index++, i0++);
ib.put(index++, inverted ? i2 : i1);
ib.put(index++, inverted ? i1 : i2);
ib.put(index++, i1++);
ib.put(index++, inverted ? i2++ : i3++);
ib.put(index++, inverted ? i3++ : i2++);
}
}
}
updateBound();
}
/**
* Updates the points array to contain the frustum corners of the given camera. The nearOverride
* and farOverride variables can be used to override the camera's near/far values with own values.
*
* <p>TODO: Reduce creation of new vectors
*
* @param viewCam
* @param nearOverride
* @param farOverride
*/
public static void updateFrustumPoints(
Camera viewCam, float nearOverride, float farOverride, float scale, Vector3f[] points) {
Vector3f pos = viewCam.getLocation();
Vector3f dir = viewCam.getDirection();
Vector3f up = viewCam.getUp();
float depthHeightRatio = viewCam.getFrustumTop() / viewCam.getFrustumNear();
float near = nearOverride;
float far = farOverride;
float ftop = viewCam.getFrustumTop();
float fright = viewCam.getFrustumRight();
float ratio = fright / ftop;
float near_height;
float near_width;
float far_height;
float far_width;
if (viewCam.isParallelProjection()) {
near_height = ftop;
near_width = near_height * ratio;
far_height = ftop;
far_width = far_height * ratio;
} else {
near_height = depthHeightRatio * near;
near_width = near_height * ratio;
far_height = depthHeightRatio * far;
far_width = far_height * ratio;
}
Vector3f right = dir.cross(up).normalizeLocal();
Vector3f temp = new Vector3f();
temp.set(dir).multLocal(far).addLocal(pos);
Vector3f farCenter = temp.clone();
temp.set(dir).multLocal(near).addLocal(pos);
Vector3f nearCenter = temp.clone();
Vector3f nearUp = temp.set(up).multLocal(near_height).clone();
Vector3f farUp = temp.set(up).multLocal(far_height).clone();
Vector3f nearRight = temp.set(right).multLocal(near_width).clone();
Vector3f farRight = temp.set(right).multLocal(far_width).clone();
points[0].set(nearCenter).subtractLocal(nearUp).subtractLocal(nearRight);
points[1].set(nearCenter).addLocal(nearUp).subtractLocal(nearRight);
points[2].set(nearCenter).addLocal(nearUp).addLocal(nearRight);
points[3].set(nearCenter).subtractLocal(nearUp).addLocal(nearRight);
points[4].set(farCenter).subtractLocal(farUp).subtractLocal(farRight);
points[5].set(farCenter).addLocal(farUp).subtractLocal(farRight);
points[6].set(farCenter).addLocal(farUp).addLocal(farRight);
points[7].set(farCenter).subtractLocal(farUp).addLocal(farRight);
if (scale != 1.0f) {
// find center of frustum
Vector3f center = new Vector3f();
for (int i = 0; i < 8; i++) {
center.addLocal(points[i]);
}
center.divideLocal(8f);
Vector3f cDir = new Vector3f();
for (int i = 0; i < 8; i++) {
cDir.set(points[i]).subtractLocal(center);
cDir.multLocal(scale - 1.0f);
points[i].addLocal(cDir);
}
}
}
private static Mesh genTangentLines(Mesh mesh, float scale) {
FloatBuffer vertexBuffer = (FloatBuffer) mesh.getBuffer(Type.Position).getData();
FloatBuffer normalBuffer = (FloatBuffer) mesh.getBuffer(Type.Normal).getData();
FloatBuffer tangentBuffer = (FloatBuffer) mesh.getBuffer(Type.Tangent).getData();
FloatBuffer binormalBuffer = null;
if (mesh.getBuffer(Type.Binormal) != null) {
binormalBuffer = (FloatBuffer) mesh.getBuffer(Type.Binormal).getData();
}
ColorRGBA originColor = ColorRGBA.White;
ColorRGBA tangentColor = ColorRGBA.Red;
ColorRGBA binormalColor = ColorRGBA.Green;
ColorRGBA normalColor = ColorRGBA.Blue;
Mesh lineMesh = new Mesh();
lineMesh.setMode(Mesh.Mode.Lines);
Vector3f origin = new Vector3f();
Vector3f point = new Vector3f();
Vector3f tangent = new Vector3f();
Vector3f normal = new Vector3f();
IntBuffer lineIndex = BufferUtils.createIntBuffer(vertexBuffer.limit() / 3 * 6);
FloatBuffer lineVertex = BufferUtils.createFloatBuffer(vertexBuffer.limit() * 4);
FloatBuffer lineColor = BufferUtils.createFloatBuffer(vertexBuffer.limit() / 3 * 4 * 4);
boolean hasParity = mesh.getBuffer(Type.Tangent).getNumComponents() == 4;
float tangentW = 1;
for (int i = 0; i < vertexBuffer.limit() / 3; i++) {
populateFromBuffer(origin, vertexBuffer, i);
populateFromBuffer(normal, normalBuffer, i);
if (hasParity) {
tangent.x = tangentBuffer.get(i * 4);
tangent.y = tangentBuffer.get(i * 4 + 1);
tangent.z = tangentBuffer.get(i * 4 + 2);
tangentW = tangentBuffer.get(i * 4 + 3);
} else {
populateFromBuffer(tangent, tangentBuffer, i);
}
int index = i * 4;
int id = i * 6;
lineIndex.put(id, index);
lineIndex.put(id + 1, index + 1);
lineIndex.put(id + 2, index);
lineIndex.put(id + 3, index + 2);
lineIndex.put(id + 4, index);
lineIndex.put(id + 5, index + 3);
setInBuffer(origin, lineVertex, index);
setInBuffer(originColor, lineColor, index);
point.set(tangent);
point.multLocal(scale);
point.addLocal(origin);
setInBuffer(point, lineVertex, index + 1);
setInBuffer(tangentColor, lineColor, index + 1);
// wvBinormal = cross(wvNormal, wvTangent) * -inTangent.w
if (binormalBuffer == null) {
normal.cross(tangent, point);
point.multLocal(-tangentW);
point.normalizeLocal();
} else {
populateFromBuffer(point, binormalBuffer, i);
}
point.multLocal(scale);
point.addLocal(origin);
setInBuffer(point, lineVertex, index + 2);
setInBuffer(binormalColor, lineColor, index + 2);
point.set(normal);
point.multLocal(scale);
point.addLocal(origin);
setInBuffer(point, lineVertex, index + 3);
setInBuffer(normalColor, lineColor, index + 3);
}
lineMesh.setBuffer(Type.Index, 1, lineIndex);
lineMesh.setBuffer(Type.Position, 3, lineVertex);
lineMesh.setBuffer(Type.Color, 4, lineColor);
lineMesh.setStatic();
// lineMesh.setInterleaved();
return lineMesh;
}
private static void processTriangleData(
Mesh mesh, List<VertexData> vertices, boolean approxTangent, boolean splitMirrored) {
ArrayList<VertexInfo> vertexMap = linkVertices(mesh, splitMirrored);
FloatBuffer tangents = BufferUtils.createFloatBuffer(vertices.size() * 4);
ColorRGBA[] cols = null;
if (debug) {
cols = new ColorRGBA[vertices.size()];
}
Vector3f tangent = new Vector3f();
Vector3f binormal = new Vector3f();
// Vector3f normal = new Vector3f();
Vector3f givenNormal = new Vector3f();
Vector3f tangentUnit = new Vector3f();
Vector3f binormalUnit = new Vector3f();
for (int k = 0; k < vertexMap.size(); k++) {
float wCoord = -1;
VertexInfo vertexInfo = vertexMap.get(k);
givenNormal.set(vertexInfo.normal);
givenNormal.normalizeLocal();
TriangleData firstTriangle = vertices.get(vertexInfo.indices.get(0)).triangles.get(0);
// check tangent and binormal consistency
tangent.set(firstTriangle.tangent);
tangent.normalizeLocal();
binormal.set(firstTriangle.binormal);
binormal.normalizeLocal();
for (int i : vertexInfo.indices) {
ArrayList<TriangleData> triangles = vertices.get(i).triangles;
for (int j = 0; j < triangles.size(); j++) {
TriangleData triangleData = triangles.get(j);
tangentUnit.set(triangleData.tangent);
tangentUnit.normalizeLocal();
if (tangent.dot(tangentUnit) < toleranceDot) {
// log.log(Level.WARNING,
// "Angle between tangents exceeds tolerance "
// + "for vertex {0}.", i);
break;
}
if (!approxTangent) {
binormalUnit.set(triangleData.binormal);
binormalUnit.normalizeLocal();
if (binormal.dot(binormalUnit) < toleranceDot) {
// log.log(Level.WARNING,
// "Angle between binormals exceeds tolerance "
// + "for vertex {0}.", i);
break;
}
}
}
}
// find average tangent
tangent.set(0, 0, 0);
binormal.set(0, 0, 0);
int triangleCount = 0;
for (int i : vertexInfo.indices) {
ArrayList<TriangleData> triangles = vertices.get(i).triangles;
triangleCount += triangles.size();
if (debug) {
cols[i] = ColorRGBA.White;
}
for (int j = 0; j < triangles.size(); j++) {
TriangleData triangleData = triangles.get(j);
tangent.addLocal(triangleData.tangent);
binormal.addLocal(triangleData.binormal);
}
}
int blameVertex = vertexInfo.indices.get(0);
if (tangent.length() < ZERO_TOLERANCE) {
log.log(Level.WARNING, "Shared tangent is zero for vertex {0}.", blameVertex);
// attempt to fix from binormal
if (binormal.length() >= ZERO_TOLERANCE) {
binormal.cross(givenNormal, tangent);
tangent.normalizeLocal();
} // if all fails use the tangent from the first triangle
else {
tangent.set(firstTriangle.tangent);
}
} else {
tangent.divideLocal(triangleCount);
}
tangentUnit.set(tangent);
tangentUnit.normalizeLocal();
if (Math.abs(Math.abs(tangentUnit.dot(givenNormal)) - 1) < ZERO_TOLERANCE) {
log.log(Level.WARNING, "Normal and tangent are parallel for vertex {0}.", blameVertex);
}
if (!approxTangent) {
if (binormal.length() < ZERO_TOLERANCE) {
log.log(Level.WARNING, "Shared binormal is zero for vertex {0}.", blameVertex);
// attempt to fix from tangent
if (tangent.length() >= ZERO_TOLERANCE) {
givenNormal.cross(tangent, binormal);
binormal.normalizeLocal();
} // if all fails use the binormal from the first triangle
else {
binormal.set(firstTriangle.binormal);
}
} else {
binormal.divideLocal(triangleCount);
}
binormalUnit.set(binormal);
binormalUnit.normalizeLocal();
if (Math.abs(Math.abs(binormalUnit.dot(givenNormal)) - 1) < ZERO_TOLERANCE) {
log.log(Level.WARNING, "Normal and binormal are parallel for vertex {0}.", blameVertex);
}
if (Math.abs(Math.abs(binormalUnit.dot(tangentUnit)) - 1) < ZERO_TOLERANCE) {
log.log(Level.WARNING, "Tangent and binormal are parallel for vertex {0}.", blameVertex);
}
}
Vector3f finalTangent = new Vector3f();
Vector3f tmp = new Vector3f();
for (int i : vertexInfo.indices) {
if (approxTangent) {
// Gram-Schmidt orthogonalize
finalTangent
.set(tangent)
.subtractLocal(tmp.set(givenNormal).multLocal(givenNormal.dot(tangent)));
finalTangent.normalizeLocal();
wCoord = tmp.set(givenNormal).crossLocal(tangent).dot(binormal) < 0f ? -1f : 1f;
tangents.put((i * 4), finalTangent.x);
tangents.put((i * 4) + 1, finalTangent.y);
tangents.put((i * 4) + 2, finalTangent.z);
tangents.put((i * 4) + 3, wCoord);
} else {
tangents.put((i * 4), tangent.x);
tangents.put((i * 4) + 1, tangent.y);
tangents.put((i * 4) + 2, tangent.z);
tangents.put((i * 4) + 3, wCoord);
// setInBuffer(binormal, binormals, i);
}
}
}
tangents.limit(tangents.capacity());
// If the model already had a tangent buffer, replace it with the regenerated one
mesh.clearBuffer(Type.Tangent);
mesh.setBuffer(Type.Tangent, 4, tangents);
if (mesh.isAnimated()) {
mesh.clearBuffer(Type.BindPoseNormal);
mesh.clearBuffer(Type.BindPosePosition);
mesh.clearBuffer(Type.BindPoseTangent);
mesh.generateBindPose(true);
}
if (debug) {
writeColorBuffer(vertices, cols, mesh);
}
mesh.updateBound();
mesh.updateCounts();
}