private float getCylinderDistance(IVector3f i_rayOrigin, IVector3f i_rayDirection) {
// if we hit one of the end caps, we are done
float d = getCapDistance(i_rayOrigin, i_rayDirection);
if (!Float.isNaN(d)) return d;
// it's much quicker to intersect with a cylinder that stands on the
// origin and extends along the positive Z axis, so translate the ray
// accordingly
Vector3f newOrigin = Draw3DCache.getVector3f();
try {
Math3D.translate(i_rayOrigin, -0.5f, -0.5f, 0, newOrigin);
// now intersect with infinite cylinder along Z axis:
float xo = newOrigin.getX();
float yo = newOrigin.getY();
float xd = i_rayDirection.getX();
float yd = i_rayDirection.getY();
float A = xd * xd + yd * yd;
float B = 2 * (xo * xd + yo * yd);
float C = xo * xo + yo * yo - 0.25f; // radius^2 = 0.25f
Math3D.solveQuadraticEquation(A, B, C, TMP_F2);
return Math3D.minDistance(TMP_F2);
} finally {
Draw3DCache.returnVector3f(newOrigin);
}
}
protected static PositionableShape createCube(
float headHeight, IVector3f size, IVector3f location) {
Position3D p3 = Position3DUtil.createAbsolutePosition();
p3.setCenter3D(new Vector3fImpl(location.getX(), location.getY(), location.getZ()));
p3.setSize3D(size);
CuboidShape shape = new CuboidShape(p3, true);
shape.setFill(true);
shape.setOutline(true);
return shape;
}
private float doGetConeDistance(
Vector3f rayOrigin, IVector3f rayDirection, float i_minZ, float i_height) {
float sa2 = m_config.getSinApex2();
float ca2 = m_config.getCosApex2();
float xo = rayOrigin.getX();
float yo = rayOrigin.getY();
float zo = rayOrigin.getZ();
float xd = rayDirection.getX();
float yd = rayDirection.getY();
float zd = rayDirection.getZ();
float xo2 = xo * xo;
float yo2 = yo * yo;
float zo2 = zo * zo;
float xd2 = xd * xd;
float yd2 = yd * yd;
float zd2 = zd * zd;
float A = ca2 * (xd2 + yd2) - sa2 * zd2;
float B = 2 * (ca2 * (xo * xd + yo * yd) - sa2 * zo * zd);
float C = ca2 * (xo2 + yo2) - sa2 * zo2;
Math3D.solveQuadraticEquation(A, B, C, TMP_F2);
// we must check the z coordinates of possible hits now
float d0 = TMP_F2[0];
float d1 = TMP_F2[1];
float zi0 = Float.isNaN(d0) ? Float.NaN : zo + zd * d0;
float zi1 = Float.isNaN(d1) ? Float.NaN : zo + zd * d1;
boolean v0 = Math3D.in(i_minZ, i_height, zi0);
boolean v1 = Math3D.in(i_minZ, i_height, zi1);
if (!v0 && !v1) return Float.NaN;
if (v0 && v1) return Math3D.minDistance(d0, d1);
if (v0) return d0;
return d1;
}