We Keep Coding

java/trig/math. relative axis of cube in 3 space, processing library(bug?)

I have a cube that rotates on its 3 axis, when key[a] == true it will spin to the left as if it was rolling that way. rotating the cube 45 degrees in any direction sets it back 90 degrees for the illusion of continuing. this maintains 3 axis that are < 45 degrees off from the environment
I believe this is correct but the x axis for the cube seems to be relative to the environment while y and z are relative to the cubes orientation, I can not find reference to this in the documentation is it a bug?
https://processing.org/reference/rotateY_.html
https://processing.org/reference/rotateX_.html
if(keys[w]) {
if (x >= 359) x = 0;
x = x + 1;
}
if(keys[a]) {
if (z >= 359) z = 0;
z = z + 1;
}
if(keys[s]) {
if (x <= 0) x = 359;
x = x - 1;
}
if(keys[d]) {
if (z <= 0) z = 359;
z = z - 1;
}
// return 90 deg for magic trick
if (x > 45 && x < 180) x = 270 + x;
if (x < 316 && x > 180) x = 360 - x;
if (y > 45 && y < 180) y = 270 + y;
if (y < 316 && y > 180) y = 360 - y;

Matrix transformations are not commutative. The order matters. The matrix operations like rotate() specify a new matrix and multiply the current matrix by the new matrix.
Hence, there is a difference between doing this
rotateX(x);
rotateY(y);
rotateZ(z);
and doing that
rotateZ(z);
rotateY(y);
rotateX(x);
And
rotateX(x1 + x2);
rotateY(y1 + y2);
rotateZ(z1 + z2);
is not the same as
rotateX(x1);
rotateY(y1);
rotateZ(z1);
rotateX(x2);
rotateY(y2);
rotateZ(z2);
One possible solution to your problem would be to use Quaternions. Quaternions behave differently than Euler angles and have also no Gimbal lock issue. Processing use OpenGL under the hood and doesn't support quaternions. However a quaternion can be transformed to a matrix and a matrix can be applied by applyMatrix().

I found this ArcBall example that dose exactly what I wanted. just added a modification to work with keys instead of mouse drag.
ArcBall with mod
// Ariel and V3ga's arcball class with a couple tiny mods by Robert Hodgin & more by me
class Arcball {
float center_x, center_y, radius;
Vec3 v_down, v_drag;
Quat q_now, q_down, q_drag;
Vec3[] axisSet;
int axis;
float mxv, myv;
float x, y;
Arcball(float center_x, float center_y, float radius){
this.center_x = center_x;
this.center_y = center_y;
this.radius = radius;
v_down = new Vec3();
v_drag = new Vec3();
q_now = new Quat();
q_down = new Quat();
q_drag = new Quat();
axisSet = new Vec3[] {new Vec3(1.0f, 0.0f, 0.0f), new Vec3(0.0f, 1.0f, 0.0f), new Vec3(0.0f, 0.0f, 1.0f)};
axis = -1; // no constraints...
}
void rollforward(){
q_down.set(q_now);
v_down = XY_to_sphere(center_x, center_y);
q_down.set(q_now);
q_drag.reset();
v_drag = XY_to_sphere(center_x, center_y - 10);
q_drag.set(Vec3.dot(v_down, v_drag), Vec3.cross(v_down, v_drag));
}
void rolldown(){
q_down.set(q_now);
v_down = XY_to_sphere(center_x, center_y);
q_down.set(q_now);
q_drag.reset();
v_drag = XY_to_sphere(center_x, center_y + 10);
q_drag.set(Vec3.dot(v_down, v_drag), Vec3.cross(v_down, v_drag));
}
void rollleft(){
q_down.set(q_now);
v_down = XY_to_sphere(center_x + radius, center_y + radius);
q_down.set(q_now);
q_drag.reset();
v_drag = XY_to_sphere(center_x + 10 * PI + radius, center_y + radius);
q_drag.set(Vec3.dot(v_down, v_drag), Vec3.cross(v_down, v_drag));
}
void rollright(){
q_down.set(q_now);
v_down = XY_to_sphere(center_x + radius, center_y + radius);
q_down.set(q_now);
q_drag.reset();
v_drag = XY_to_sphere(center_x - 10 * PI + radius, center_y + radius);
q_drag.set(Vec3.dot(v_down, v_drag), Vec3.cross(v_down, v_drag));
}
void mousePressed(){
v_down = XY_to_sphere(mouseX, mouseY); // when m pressed
q_down.set(q_now);
q_drag.reset();
}
void mouseDragged(){
v_drag = XY_to_sphere(mouseX, mouseY);
q_drag.set(Vec3.dot(v_down, v_drag), Vec3.cross(v_down, v_drag));
}
void run(){
q_now = Quat.mul(q_drag, q_down);
applyQuat2Matrix(q_now);
x += mxv;
y += myv;
mxv -= mxv * .01;
myv -= myv * .01;
}
Vec3 XY_to_sphere(float x, float y){
Vec3 v = new Vec3();
v.x = (x - center_x) / radius;
v.y = (y - center_y) / radius;
float mag = v.x * v.x + v.y * v.y;
if (mag > 1.0f){
v.normalize();
} else {
v.z = sqrt(1.0f - mag);
}
return (axis == -1) ? v : constrain_vector(v, axisSet[axis]);
}
Vec3 constrain_vector(Vec3 vector, Vec3 axis){
Vec3 res = new Vec3();
res.sub(vector, Vec3.mul(axis, Vec3.dot(axis, vector)));
res.normalize();
return res;
}
void applyQuat2Matrix(Quat q){
// instead of transforming q into a matrix and applying it...
float[] aa = q.getValue();
rotate(aa[0], aa[1], aa[2], aa[3]);
}
}
static class Vec3{
float x, y, z;
Vec3(){
}
Vec3(float x, float y, float z){
this.x = x;
this.y = y;
this.z = z;
}
void normalize(){
float length = length();
x /= length;
y /= length;
z /= length;
}
float length(){
return (float) Math.sqrt(x * x + y * y + z * z);
}
static Vec3 cross(Vec3 v1, Vec3 v2){
Vec3 res = new Vec3();
res.x = v1.y * v2.z - v1.z * v2.y;
res.y = v1.z * v2.x - v1.x * v2.z;
res.z = v1.x * v2.y - v1.y * v2.x;
return res;
}
static float dot(Vec3 v1, Vec3 v2){
return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
}
static Vec3 mul(Vec3 v, float d){
Vec3 res = new Vec3();
res.x = v.x * d;
res.y = v.y * d;
res.z = v.z * d;
return res;
}
void sub(Vec3 v1, Vec3 v2){
x = v1.x - v2.x;
y = v1.y - v2.y;
z = v1.z - v2.z;
}
}
static class Quat{
float w, x, y, z;
Quat(){
reset();
}
Quat(float w, float x, float y, float z){
this.w = w;
this.x = x;
this.y = y;
this.z = z;
}
void reset(){
w = 1.0f;
x = 0.0f;
y = 0.0f;
z = 0.0f;
}
void set(float w, Vec3 v){
this.w = w;
x = v.x;
y = v.y;
z = v.z;
}
void set(Quat q){
w = q.w;
x = q.x;
y = q.y;
z = q.z;
}
static Quat mul(Quat q1, Quat q2){
Quat res = new Quat();
res.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
res.x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
res.y = q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z;
res.z = q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x;
return res;
}
float[] getValue(){
// transforming this quat into an angle and an axis vector...
float[] res = new float[4];
float sa = (float) Math.sqrt(1.0f - w * w);
if (sa < EPSILON){
sa = 1.0f;
}
res[0] = (float) Math.acos(w) * 2.0f;
res[1] = x / sa;
res[2] = y / sa;
res[3] = z / sa;
return res;
}
}
main
Arcball arcball;
//framecount
int fcount, lastm;
float frate;
int fint = 3;
boolean[] keys = new boolean[4];
final int w = 0;
final int s = 1;
final int a = 2;
final int d = 3;
void setup() {
size(900, 700, P3D);
frameRate(60);
noStroke();
arcball = new Arcball(width/2, height/2+100, 360);
}
void draw() {
lights();
background(255,160,122);
if(keys[w]) { arcball.rollforward(); }
if(keys[a]) { arcball.rollleft(); }
if(keys[s]) { arcball.rolldown(); }
if(keys[d]) { arcball.rollright(); }
ambient(80);
lights();
translate(width/2, height/2-100, 0);
box(50);
translate(0, 200, 0);
arcball.run();
box(50);
}
void keyPressed() {
switch(key) {
case 97:
keys[a] = true;
break;
case 100:
keys[d] = true;
break;
case 115:
keys[s] = true;
break;
case 119:
keys[w] = true;
break;
}
}
void keyReleased() {
switch(key) {
case 97:
keys[a] = false;
break;
case 100:
keys[d] = false;
break;
case 115:
keys[s] = false;
break;
case 119:
keys[w] = false;
break;
}
}
will add support for multiple keys at once later with an edit.... stay tuned

Centre of mass of a random 3D polygon (obj file or stl file)

Currently I have an ArrayList of vertices in a 3-dimensional cartesian coordinates system. The polygon is random. It can be a car, a cup or even a dragon.
Assuming the density does not change, how to calculate the centre of mass (x,y,z) of this 3D object?
I am storing the faces and vertices in ArrayList.
public ArrayList<stlFace> StlFaces = new ArrayList<stlFace>();
public ArrayList<VertexGeometric> VertexList = new ArrayList<VertexGeometric>();

I was using this for calculating surface which is proportional to mass of each face or triangle. And to calculate center off mass of each triangle and center of mass of whole object I was using this. I added helper methods getCenter() and getSurface() to Face class to encapsulate calculations specific to just one face/triangle.
public static class Vertex {
public float x = 0;
public float y = 0;
public float z = 0;
public Vertex(float x, float y, float z) {
this.x = x;
this.y = y;
this.z = z;
}
}
public static class Face {
public Vertex v1;
public Vertex v2;
public Vertex v3;
public Face(Vertex v1, Vertex v2, Vertex v3) {
this.v1 = v1;
this.v2 = v2;
this.v3 = v3;
}
public Vertex getCenter() {
Vertex triangleCenter = new Vertex(0, 0, 0);
triangleCenter.x += v1.x;
triangleCenter.x += v2.x;
triangleCenter.x += v3.x;
triangleCenter.y += v1.y;
triangleCenter.y += v2.y;
triangleCenter.y += v3.y;
triangleCenter.z += v1.z;
triangleCenter.z += v2.z;
triangleCenter.z += v3.z;
triangleCenter.x /= 3;
triangleCenter.y /= 3;
triangleCenter.z /= 3;
return triangleCenter;
}
public float getSurface() {
float x1 = v1.x - v2.x;
float x2 = v1.y - v2.y;
float x3 = v1.z - v2.z;
float y1 = v1.x - v3.x;
float y2 = v1.y - v3.y;
float y3 = v1.z - v3.z;
return (float) Math.sqrt(
Math.pow(x2 * y3 - x3 * y2, 2) +
Math.pow(x3 * y1 - x1 * y3, 2) +
Math.pow(x1 * y2 - x2 * y1, 2)
) / 2f;
}
}
public static Vertex calculateMassCenter(List<Face> faces) {
Vertex massCenter = new Vertex(0, 0, 0);
float mass = 0;
for (Face face : faces) {
Vertex triangleCenter = face.getCenter();
float faceMass = face.getSurface();
mass += faceMass;
massCenter.x += faceMass * triangleCenter.x;
massCenter.y += faceMass * triangleCenter.y;
massCenter.z += faceMass * triangleCenter.z;
}
massCenter.x /= mass;
massCenter.y /= mass;
massCenter.z /= mass;
return massCenter;
}

Result of Quaternion Multiplication not yielding expected Rotation of local coordinate system

I'm getting unexpected results when multiplying two quaternions and applying the resulting rotation to my local right-handed coordinate system. (X pointing forward, Y to the right and Z downward).
(See my Java SCCE below)
So I am trying to first apply a Z rotation by 90 degrees (yaw) and then a rotation of 90 degrees around the local X axis (roll).
I am trying to accomplish this by multiplying two quaternions representing these two rotations, creating a rotation Matrix from the result and applying it to the 3 unit vectors of my coordinate system but the results I am getting do not make sense. (i.e. they do not represent the coordinate system you should get from these two rotations.)
I have tried changing the quaternion multiplication order which did not help (see code lines that were commented out in the main method of the SCCE).
I have also tried creating the quaternion for the second rotation from global Y to simulate that it was created from the resulting local coordinate system after the first rotation.
For reference I am also calculating the result by applying the two individual rotation matrices (which works as expected).
What am I doing wrong?
import java.text.DecimalFormat;
import java.text.NumberFormat;
public class Quaternion {
public static final double NORMALIZATION_LOWER_TOLERANCE = 1 - 1e-4;
public static final double NORMALIZATION_UPPER_TOLERANCE = 1 + 1e-4;
private double w = 1.0;
private double x = 0.0;
private double y = 0.0;
private double z = 0.0;
public static void main(String[] args) {
Vector3D xVect = new Vector3D(1,0,0);
Vector3D yVect = new Vector3D(0,1,0);
Vector3D zVect = new Vector3D(0,0,1);
System.out.println("Initial Local Coordinate System: X:"+xVect+" / Y:"+yVect+ " / Z:"+zVect);
Quaternion rotZ = new Quaternion(Math.PI/2, zVect); // Yaw +90 deg
Quaternion rotY = new Quaternion(Math.PI/2, yVect); // Yaw +90 deg
Quaternion rotX = new Quaternion(Math.PI/2, xVect); // Then roll +90 deg
Matrix rotationMatrixZ = new Matrix(rotZ);
Vector3D localX = xVect.rotate(rotationMatrixZ);
Vector3D localY = yVect.rotate(rotationMatrixZ);
Vector3D localZ = zVect.rotate(rotationMatrixZ);
System.out.println("New Local Coordinate System after Yaw: X:"+localX+" / Y:"+localY+ " / Z:"+localZ); // Gives expected result
Quaternion localRotX = new Quaternion(Math.PI/2, localX);
Matrix localRotXMatrix = new Matrix(localRotX);
Vector3D rotatedX = localX.rotate(localRotXMatrix);
Vector3D rotatedY = localY.rotate(localRotXMatrix);
Vector3D rotatedZ = localZ.rotate(localRotXMatrix);
System.out.println("New Local Coordinate System two local rotations: X:"+rotatedX+" / Y:"+rotatedY+ " / Z:"+rotatedZ); // Gives expected result
Quaternion rotZX = rotZ.multiply(rotX);
// Quaternion rotZX = rotX.multiply(rotZ); // Tried both orders
// Quaternion rotZX = rotZ.multiply(rotY); // rotY is in fact the local rotX
// Quaternion rotZX = rotZ.multiply(rotY); // rotY is in fact the local rotX, tried both orders
rotZX.normalizeIfNeeded();
Matrix rotationXMatrixZX = new Matrix(rotZX);
rotatedX = xVect.rotate(rotationXMatrixZX);
rotatedY = localY.rotate(rotationXMatrixZX);
rotatedZ = localZ.rotate(rotationXMatrixZX);
System.out.println("New Local Coordinate System Quaternion Multiplication: X:"+rotatedX+" / Y:"+rotatedY+ " / Z:"+rotatedZ); // Expect same as above
}
public Quaternion() {
}
public Quaternion(double w, double x, double y, double z) {
this.w = w;
this.x = x;
this.y = y;
this.z = z;
}
public Quaternion(double angle, Vector3D vector){
double halfAngle = angle / 2;
double sin = Math.sin(halfAngle);
this.w = Math.cos(halfAngle);
this.x = vector.getX()*sin;
this.y = vector.getY()*sin;
this.z = vector.getZ()*sin;
}
public boolean normalizeIfNeeded() {
double sum = w * w + x * x + y * y + z * z;
if (NORMALIZATION_LOWER_TOLERANCE < sum && sum < NORMALIZATION_UPPER_TOLERANCE) {
return false;
}
double magnitude = Math.sqrt(sum);
w /= magnitude;
x /= magnitude;
y /= magnitude;
z /= magnitude;
return true;
}
public Quaternion multiply(Quaternion q2) {
Quaternion result = new Quaternion();
result.w = w * q2.w - x * q2.x - y * q2.y - z * q2.z;
result.x = w * q2.x + x * q2.w + y * q2.z - z * q2.y;
result.y = w * q2.y - x * q2.z + y * q2.w + z * q2.x;
result.z = w * q2.z + x * q2.y - y * q2.x + z * q2.w;
return result;
}
public Quaternion conjugate() {
return new Quaternion(w, -x, -y, -z);
}
public double getW() {
return w;
}
public double getX() {
return x;
}
public double getY() {
return y;
}
public double getZ() {
return z;
}
#Override
public String toString() {
return "Quaternion [w=" + w + ", x=" + x + ", y=" + y + ", z=" + z + "]";
}
static class Vector3D {
double x=0;
double y=0;
double z=0;
public Vector3D(double x, double y, double z) {
this.x = x;
this.y = y;
this.z = z;
}
public Vector3D rotate(Matrix rotationMatrix){
return rotationMatrix.multiply(this);
}
public double getX() {
return x;
}
public double getY() {
return y;
}
public double getZ() {
return z;
}
#Override
public String toString() {
NumberFormat df = DecimalFormat.getNumberInstance();
return "[x=" + df.format(x) + ", y=" + df.format(y) + ", z=" + df.format(z) + "]";
}
}
static class Matrix {
private double[][] values;
public Matrix(int rowCount, int colCount) {
values = new double[rowCount][colCount];
}
public Matrix(Quaternion quaternionForRotationMatrix) {
this(3,3);
double w = quaternionForRotationMatrix.getW();
double x = quaternionForRotationMatrix.getX();
double y = quaternionForRotationMatrix.getY();
double z = quaternionForRotationMatrix.getZ();
double ww = w*w;
double wx = w*x;
double xx = x*x;
double xy = x*y;
double xz = x*z;
double wy = w*y;
double yy = y*y;
double yz = y*z;
double wz = w*z;
double zz = z*z;
values[0][0] = ww + xx - yy - zz;
values[0][1] = 2 * xy - 2 * wz;
values[0][2] = 2 * xz + 2 * wy;
values[1][0] = 2 * xy + 2 * wz;
values[1][1] = ww - xx + yy - zz;
values[1][2] = 2 * yz + 2 * wx;
values[2][0] = 2 * xz - 2 * wy;
values[2][1] = 2 * yz - 2 * wx;
values[2][2] = ww - xx - yy + zz;
}
public Vector3D multiply(Vector3D vector){
double [][] vect = new double [3][1];
vect[0][0] = vector.getX();
vect[1][0] = vector.getY();
vect[2][0] = vector.getZ();
double [][] result = multiplyMatrices(values, vect);
return new Vector3D(result[0][0], result[1][0], result[2][0]);
}
private double[][] multiplyMatrices(double[][] m1, double[][] m2) {
double[][] result = null;
if (m1[0].length == m2.length) {
int rowCount1 = m1.length;
int colCount1 = m1[0].length;
int rowCount2 = m2[0].length;
result = new double[rowCount1][rowCount2];
for (int i = 0; i < rowCount1; i++) {
for (int j = 0; j < rowCount2; j++) {
result[i][j] = 0;
for (int k = 0; k < colCount1; k++) {
result[i][j] += m1[i][k] * m2[k][j];
}
}
}
} else {
int rowCount = m1.length;
int colCount = m1[0].length;
result = new double[rowCount][colCount];
for (int i = 0; i < m1.length; i++) {
for (int j = 0; j < m1[0].length; j++) {
result[i][j] = 0;
}
}
}
return result;
}
#Override
public String toString() {
StringBuffer sb = new StringBuffer("Matrix = ");
for(int row = 0 ; row<values.length; row++){
sb.append ("[ ");
for(int col = 0 ; col<values[0].length; col++){
sb.append(Double.toString(values[row][col]));
if(col<values.length-1){
sb.append(" | ");
}
}
sb.append("] ");
}
return sb.toString();
}
}
}

Nevermind. Found it. I had an error in the formulas to build the rotation matrix. It now works as expected.
I am making a mental note to use formulas from Wikipedia in the future and not some random other site.
The respective part should be
values[0][0] = ww + xx - yy - zz;
values[0][1] = 2 * xy - 2 * wz;
values[0][2] = 2 * xz + 2 * wy;
values[1][0] = 2 * xy + 2 * wz;
values[1][1] = ww - xx + yy - zz;
values[1][2] = 2 * yz - 2 * wx; //CORRECTED SIGN
values[2][0] = 2 * xz - 2 * wy;
values[2][1] = 2 * yz + 2 * wx; //CORRECTED SIGN
values[2][2] = ww - xx - yy + zz;
At the end of the main method I was also using the wrong vectors for y and z:
Matrix rotationXMatrixZX = new Matrix(rotZX);
rotatedX = xVect.rotate(rotationXMatrixZX);
rotatedY = yVect.rotate(rotationXMatrixZX); // Corrected used y-vector
rotatedZ = zVect.rotate(rotationXMatrixZX); // Corrected used z-vector

Point of contact of 2 OBBs?

I'm working on the physics for my GTA2-like game so I can learn more about game physics.
The collision detection and resolution are working great.
I'm now just unsure how to compute the point of contact when I hit a wall.
Here is my OBB class:
public class OBB2D
{
private Vector2D projVec = new Vector2D();
private static Vector2D projAVec = new Vector2D();
private static Vector2D projBVec = new Vector2D();
private static Vector2D tempNormal = new Vector2D();
private Vector2D deltaVec = new Vector2D();
// Corners of the box, where 0 is the lower left.
private Vector2D corner[] = new Vector2D[4];
private Vector2D center = new Vector2D();
private Vector2D extents = new Vector2D();
private RectF boundingRect = new RectF();
private float angle;
//Two edges of the box extended away from corner[0].
private Vector2D axis[] = new Vector2D[2];
private double origin[] = new double[2];
public OBB2D(float centerx, float centery, float w, float h, float angle)
{
for(int i = 0; i < corner.length; ++i)
{
corner[i] = new Vector2D();
}
for(int i = 0; i < axis.length; ++i)
{
axis[i] = new Vector2D();
}
set(centerx,centery,w,h,angle);
}
public OBB2D(float left, float top, float width, float height)
{
for(int i = 0; i < corner.length; ++i)
{
corner[i] = new Vector2D();
}
for(int i = 0; i < axis.length; ++i)
{
axis[i] = new Vector2D();
}
set(left + (width / 2), top + (height / 2),width,height,0.0f);
}
public void set(float centerx,float centery,float w, float h,float angle)
{
float vxx = (float)Math.cos(angle);
float vxy = (float)Math.sin(angle);
float vyx = (float)-Math.sin(angle);
float vyy = (float)Math.cos(angle);
vxx *= w / 2;
vxy *= (w / 2);
vyx *= (h / 2);
vyy *= (h / 2);
corner[0].x = centerx - vxx - vyx;
corner[0].y = centery - vxy - vyy;
corner[1].x = centerx + vxx - vyx;
corner[1].y = centery + vxy - vyy;
corner[2].x = centerx + vxx + vyx;
corner[2].y = centery + vxy + vyy;
corner[3].x = centerx - vxx + vyx;
corner[3].y = centery - vxy + vyy;
this.center.x = centerx;
this.center.y = centery;
this.angle = angle;
computeAxes();
extents.x = w / 2;
extents.y = h / 2;
computeBoundingRect();
}
//Updates the axes after the corners move. Assumes the
//corners actually form a rectangle.
private void computeAxes()
{
axis[0].x = corner[1].x - corner[0].x;
axis[0].y = corner[1].y - corner[0].y;
axis[1].x = corner[3].x - corner[0].x;
axis[1].y = corner[3].y - corner[0].y;
// Make the length of each axis 1/edge length so we know any
// dot product must be less than 1 to fall within the edge.
for (int a = 0; a < axis.length; ++a)
{
float l = axis[a].length();
float ll = l * l;
axis[a].x = axis[a].x / ll;
axis[a].y = axis[a].y / ll;
origin[a] = corner[0].dot(axis[a]);
}
}
public void computeBoundingRect()
{
boundingRect.left = JMath.min(JMath.min(corner[0].x, corner[3].x), JMath.min(corner[1].x, corner[2].x));
boundingRect.top = JMath.min(JMath.min(corner[0].y, corner[1].y),JMath.min(corner[2].y, corner[3].y));
boundingRect.right = JMath.max(JMath.max(corner[1].x, corner[2].x), JMath.max(corner[0].x, corner[3].x));
boundingRect.bottom = JMath.max(JMath.max(corner[2].y, corner[3].y),JMath.max(corner[0].y, corner[1].y));
}
public void set(RectF rect)
{
set(rect.centerX(),rect.centerY(),rect.width(),rect.height(),0.0f);
}
// Returns true if other overlaps one dimension of this.
private boolean overlaps1Way(OBB2D other)
{
for (int a = 0; a < axis.length; ++a) {
double t = other.corner[0].dot(axis[a]);
// Find the extent of box 2 on axis a
double tMin = t;
double tMax = t;
for (int c = 1; c < corner.length; ++c) {
t = other.corner[c].dot(axis[a]);
if (t < tMin) {
tMin = t;
} else if (t > tMax) {
tMax = t;
}
}
// We have to subtract off the origin
// See if [tMin, tMax] intersects [0, 1]
if ((tMin > 1 + origin[a]) || (tMax < origin[a])) {
// There was no intersection along this dimension;
// the boxes cannot possibly overlap.
return false;
}
}
// There was no dimension along which there is no intersection.
// Therefore the boxes overlap.
return true;
}
public void moveTo(float centerx, float centery)
{
float cx,cy;
cx = center.x;
cy = center.y;
deltaVec.x = centerx - cx;
deltaVec.y = centery - cy;
for (int c = 0; c < 4; ++c)
{
corner[c].x += deltaVec.x;
corner[c].y += deltaVec.y;
}
boundingRect.left += deltaVec.x;
boundingRect.top += deltaVec.y;
boundingRect.right += deltaVec.x;
boundingRect.bottom += deltaVec.y;
this.center.x = centerx;
this.center.y = centery;
computeAxes();
}
// Returns true if the intersection of the boxes is non-empty.
public boolean overlaps(OBB2D other)
{
if(right() < other.left())
{
return false;
}
if(bottom() < other.top())
{
return false;
}
if(left() > other.right())
{
return false;
}
if(top() > other.bottom())
{
return false;
}
if(other.getAngle() == 0.0f && getAngle() == 0.0f)
{
return true;
}
return overlaps1Way(other) && other.overlaps1Way(this);
}
public Vector2D getCenter()
{
return center;
}
public float getWidth()
{
return extents.x * 2;
}
public float getHeight()
{
return extents.y * 2;
}
public void setAngle(float angle)
{
set(center.x,center.y,getWidth(),getHeight(),angle);
}
public float getAngle()
{
return angle;
}
public void setSize(float w,float h)
{
set(center.x,center.y,w,h,angle);
}
public float left()
{
return boundingRect.left;
}
public float right()
{
return boundingRect.right;
}
public float bottom()
{
return boundingRect.bottom;
}
public float top()
{
return boundingRect.top;
}
public RectF getBoundingRect()
{
return boundingRect;
}
public boolean overlaps(float left, float top, float right, float bottom)
{
if(right() < left)
{
return false;
}
if(bottom() < top)
{
return false;
}
if(left() > right)
{
return false;
}
if(top() > bottom)
{
return false;
}
return true;
}
public static float distance(float ax, float ay,float bx, float by)
{
if (ax < bx)
return bx - ay;
else
return ax - by;
}
public Vector2D project(float ax, float ay)
{
projVec.x = Float.MAX_VALUE;
projVec.y = Float.MIN_VALUE;
for (int i = 0; i < corner.length; ++i)
{
float dot = Vector2D.dot(corner[i].x,corner[i].y,ax,ay);
projVec.x = JMath.min(dot, projVec.x);
projVec.y = JMath.max(dot, projVec.y);
}
return projVec;
}
public Vector2D getCorner(int c)
{
return corner[c];
}
public int getNumCorners()
{
return corner.length;
}
public static float collisionResponse(OBB2D a, OBB2D b, Vector2D outNormal)
{
float depth = Float.MAX_VALUE;
for (int i = 0; i < a.getNumCorners() + b.getNumCorners(); ++i)
{
Vector2D edgeA;
Vector2D edgeB;
if(i >= a.getNumCorners())
{
edgeA = b.getCorner((i + b.getNumCorners() - 1) % b.getNumCorners());
edgeB = b.getCorner(i % b.getNumCorners());
}
else
{
edgeA = a.getCorner((i + a.getNumCorners() - 1) % a.getNumCorners());
edgeB = a.getCorner(i % a.getNumCorners());
}
tempNormal.x = edgeB.x -edgeA.x;
tempNormal.y = edgeB.y - edgeA.y;
tempNormal.normalize();
projAVec.equals(a.project(tempNormal.x,tempNormal.y));
projBVec.equals(b.project(tempNormal.x,tempNormal.y));
float distance = OBB2D.distance(projAVec.x, projAVec.y,projBVec.x,projBVec.y);
if (distance > 0.0f)
{
return 0.0f;
}
else
{
float d = Math.abs(distance);
if (d < depth)
{
depth = d;
outNormal.equals(tempNormal);
}
}
}
float dx,dy;
dx = b.getCenter().x - a.getCenter().x;
dy = b.getCenter().y - a.getCenter().y;
float dot = Vector2D.dot(dx,dy,outNormal.x,outNormal.y);
if(dot > 0)
{
outNormal.x = -outNormal.x;
outNormal.y = -outNormal.y;
}
return depth;
}
public Vector2D getMoveDeltaVec()
{
return deltaVec;
}
};

I'm now just unsure how to compute the point of contact when I hit a
wall.
You can represent a wall with a simple plane.
The OBB-vs-plane intersection test is the simplest separating axis test of them all:
If two convex objects don't intersect, then there is a plane where
the projection of these two objects will not intersect.
A box intersects plane only if the plane normal forms a separating axis. Compute the projection of the box center and the projected radius (4 dot products and a few adds) and you're good to go (you also get penetration depth for free).
The condition looks as follows:
|d| <= a1|n*A1| + a2|n*A2| + a3|n*A3|
Here:
d distance from the center of the box to the plane.
a1...a3 the extents of the box from the center.
n normal of the plane
A1...A3 the x,y,z-axis of the box
Some pseudocode:
//Test if OBB b intersects plane p
int TestOBBPlane(OBB b, Plane p)
{
// Compute the projection interval radius of b onto L(t) = b.c + t * p.n
float r = b.e[0]*Abs(Dot(p.n, b.u[0])) +
b.e[1]*Abs(Dot(p.n, b.u[1])) +
b.e[2]*Abs(Dot(p.n, b.u[2]));
// Compute distance of box center from plane
float s = Dot(p.n, b.c) – p.d;
// Intersection occurs when distance s falls within [-r,+r] interval
return Abs(s) <= r;
}
The OBB-vs-OBB intersection test is more complicated.
Let us refer to this great tutorial:
In this case we no longer have corresponding separating lines that are
perpendicular to the separating axes. Instead, we have separating
planes that separate the bounding volumes (and they are perpendicular
to their corresponding separating axes).
In 3D space, each OBB only has 3 unique planes extended by its faces,
and the separating planes are parallel to these faces. We are
interested in the separating planes parallel to the faces, but in 3D
space, the faces are not the only concern. We are also interested in
the edges. The separating planes of interest are parallel to the faces
of the boxes, and the separating axes of interest are perpendicular to
the separating planes. Hence the separating axes of interest are
perpendicular to the 3 unique faces of each box. Notice these 6
separating axes of interest correspond to the 6 local (XYZ) axes of
the two boxes.
So there are 9 separating axes to consider for edges collision in
addition to the 6 separating axes we already have found for the faces
collision. This makes the total number of possible separating axes to
consider at 15.
Here are the 15 possible separating axes (L) you will need to test:
CASE 1: L = Ax
CASE 2: L = Ay
CASE 3: L = Az
CASE 4: L = Bx
CASE 5: L = By
CASE 6: L = Bz
CASE 7: L = Ax x Bx
CASE 8: L = Ax x By
CASE 9: L = Ax x Bz
CASE 10: L = Ay x Bx
CASE 11: L = Ay x By
CASE 12: L = Ay x Bz
CASE 13: L = Az x Bx
CASE 14: L = Az x By
CASE 15: L = Az x Bz
Here:
Ax unit vector representing the x-axis of A
Ay unit vector representing the y-axis of A
Az unit vector representing the z-axis of A
Bx unit vector representing the x-axis of B
By unit vector representing the y-axis of B
Bz unit vector representing the z-axis of B
Now you can see the algorithm behind the OBB-OBB intersection test.
Let's jump to the source code:
2D OBB-OBB: http://www.flipcode.com/archives/2D_OBB_Intersection.shtml
3D OBB-OBB: http://www.geometrictools.com/LibMathematics/Intersection/Intersection.html
P.S: This link http://www.realtimerendering.com/intersections.html will be useful to those, who wish to go beyond planes and boxes.

Calculating Geometric median of 2D points

I am calculating Geometric median of some (x,y) points in java. To calculate Geometric median, first i am calculating centroid of all the points, then this centroid is used to calculate Geometric median. My code works fine, but sometimes it goes to an infinite loop (i think.). The problem is with my while condition. This while condition should be change according to input points, but i don't know how. Below I am putting the complete code.
import java.util.ArrayList;
public class GeometricMedian {
private static ArrayList<Point> points = new ArrayList<Point>();
private class Point {
private double x;
private double y;
Point(double a, double b) {
x = a;
y = b;
}
}
public static void main(String[] args) {
GeometricMedian gm = new GeometricMedian();
gm.addPoints();
Point centroid = gm.getCentroid();
Point geoMedian = gm.getGeoMedian(centroid);
System.out.println("GeometricMedian= {" + (float) geoMedian.x + ", "
+ (float) geoMedian.y + "}");
}
public void addPoints() {
points.add(new Point(0, 1));
points.add(new Point(2, 5));
points.add(new Point(3, 1));
points.add(new Point(4, 0));
}
public Point getCentroid() {
double cx = 0.0D;
double cy = 0.0D;
for (int i = 0; i < points.size(); i++) {
Point pt = points.get(i);
cx += pt.x;
cy += pt.y;
}
return new Point(cx / points.size(), cy / points.size());
}
public Point getGeoMedian(Point start) {
double cx = 0;
double cy = 0;
double centroidx = start.x;
double centroidy = start.y;
do {
double totalWeight = 0;
for (int i = 0; i < points.size(); i++) {
Point pt = points.get(i);
double weight = 1 / distance(pt.x, pt.y, centroidx, centroidy);
cx += pt.x * weight;
cy += pt.y * weight;
totalWeight += weight;
}
cx /= totalWeight;
cy /= totalWeight;
} while (Math.abs(cx - centroidx) > 0.5
|| Math.abs(cy - centroidy) > 0.5);// Probably this condition
// needs to change
return new Point(cx, cy);
}
private static double distance(double x1, double y1, double x2, double y2) {
x1 -= x2;
y1 -= y2;
return Math.sqrt(x1 * x1 + y1 * y1);
}
}
Please help me to fix the bug, also if there exitis any better way to calculate Geometric median of some 2D points, write here. Thank you.

I don't see why you need two loops. You only need the loop over all the points. What is the reason for the other one, in your view?

One way to solve this is to iterate certain number of times. This similar to K-Means method where it either converges to a specific threshold or stops after a predefined number of iterations.