Develop Reference

Detecting costs/variables resulting in unbounded optimization problem in ojAlgo

I am using the ojAlgo linear/quadratic solver via ExpressionsBasedModel to solve the layout of graphical elements in a plotting library so that they fit neatly into the screen boundaries. Specifically, I want to solve for scale and translation so that the coordinates of a scatter plot fill up the screen space. I do that by declaring scale and translation variables of the ExpressionsBasedModel and transform the scatter plot coordinates to the screen using those variables and then construct linear constraints that the transformed coordinates should project inside the screen. I also add a negative cost to the scale variables, so that they are maximized and the scatter plot covers as much screen space as possible. My problem is that in some special cases, for example if I have only one point to plot, this results in an unbounded problem where the scale goes towards infinity without any constraint being active. How can I detect the scale variables for which this would happen and fix them to some default values?
To illustrate the above problem, I constructed a toy plotting library (the full library that I am working on is too big to fit in this question). To help layout the graphical elements, I have a problem class:
class Problem {
private ArrayList<Variable> _scale_variables = new ArrayList<Variable>();
private ExpressionsBasedModel _model = new ExpressionsBasedModel();
Variable freeVariable() {
return _model.addVariable();
}
Variable scaleVariable() {
Variable x = _model.addVariable();
x.lower(0.0); // Negative scale not allowed
_scale_variables.add(x);
return x;
}
Expression expr() {
return _model.addExpression();
}
Result solve() {
for (Variable scale_var: _scale_variables) {
// This is may result in unbounded solution for degenerate cases.
Expression expr = _model.addExpression("Encourage-larger-scale");
expr.set(scale_var, -1.0);
expr.weight(1.0);
}
return _model.minimise();
}
}
It wraps an ExpressionsBasedModel and has some facilities to create variables. For the transform that I will use to map my scatter point coordinates to screen coordinates, I have this class:
class Transform2d {
Variable x_scale;
Variable y_scale;
Variable x_translation;
Variable y_translation;
Transform2d(Problem problem) {
x_scale = problem.scaleVariable();
y_scale = problem.scaleVariable();
x_translation = problem.freeVariable();
y_translation = problem.freeVariable();
}
void respectBounds(double x, double y, double marker_size,
double width, double height,
Problem problem) {
// Respect left and right screen bounds
{
Expression expr = problem.expr();
expr.set(x_scale, x);
expr.set(x_translation, 1.0);
expr.lower(marker_size);
expr.upper(width - marker_size);
}
// Respect top and bottom screen bounds
{
Expression expr = problem.expr();
expr.set(y_scale, y);
expr.set(y_translation, 1.0);
expr.lower(marker_size);
expr.upper(height - marker_size);
}
}
}
The respectBounds method is used to add the constraints of a single point in the scatter plot the the Problem class mentioned before. To add all the points of a scatter plot, I have this function:
void addScatterPoints(
double[] xy_pairs,
// How much space every marker occupies
double marker_size,
Transform2d transform_to_screen,
// Screen size
double width, double height,
Problem problem) {
int data_count = xy_pairs.length/2;
for (int i = 0; i < data_count; i++) {
int offset = 2*i;
double x = xy_pairs[offset + 0];
double y = xy_pairs[offset + 1];
transform_to_screen.respectBounds(x, y, marker_size, width, height, problem);
}
}
First, let's look at what a non-degenerate case looks like. I specify the screen size and the size of the markers used for the scatter plot. I also specify the data to plot, build the problem and solve it. Here is the code
Problem problem = new Problem();
double marker_size = 4;
double width = 800;
double height = 600;
double[] data_to_plot = new double[] {
1.0, 2.0,
4.0, 9.3,
7.0, 4.5};
Transform2d transform = new Transform2d(problem);
addScatterPoints(data_to_plot, marker_size, transform, width, height, problem);
Result result = problem.solve();
System.out.println("Solution: " + result);
which prints out Solution: OPTIMAL -81.0958904109589 # { 0, 81.0958904109589, 795.99999999999966, -158.19178082191794 }.
This is what a degenerate case looks like, plotting two points with the same y-coordinate:
Problem problem = new Problem();
double marker_size = 4;
double width = 800;
double height = 600;
double[] data_to_plot = new double[] {
1, 1,
9, 1
};
Transform2d transform = new Transform2d(problem);
addScatterPoints(data_to_plot, marker_size, transform, width, height, problem);
Result result = problem.solve();
System.out.println("Solution: " + result);
It displays Solution: UNBOUNDED -596.0 # { 88.44444444444444, 596, 0, 0 }.
As mentioned before, my question is: How can I detect the scale variables whose negative cost would result in an unbounded solution and constraint them to some default value, so that my solution is not unbounded?

OpenGL - difficulty with constructing shapes

I'm starting work on a simple shape-batching system for my 3D engine that will enable me to draw lines and rectangles, etc... with a lower draw call count. I think I've got the basic ideas figured out for the most part, but I'm having problems when I try to draw multiple objects (currently just lines with a thickness you can specify).
Here's a screenshot to show you what I mean:
I'm using indexed rendering with glDrawElements, and two VBOs to represent the vertex data - one for positions, and one for colours.
I construct a line for my shape-batcher by specifying start and end points, like so:
shapeRenderer.begin();
shapeRenderer.setViewMatrix(viewMatrix);
shapeRenderer.setProjectionMatrix(projectionMatrix);
shapeRenderer.setCurrentColour(0, 1f, 0);
shapeRenderer.drawLine(2, 2, 5, 2);
shapeRenderer.setCurrentColour(0, 1f, 1f);
shapeRenderer.drawLine(2, 5, 5, 5);
shapeRenderer.end();
The first line, represented in green in the screenshot, shows perfectly. If I draw only one line it's completely fine. If I were to draw only the second line it would show perfectly as well.
When I call drawLine the following code executes, which I use to compute directions and normals:
private Vector2f temp2fA = new Vector2f();
private Vector2f temp2fB = new Vector2f();
private Vector2f temp2fDir = new Vector2f();
private Vector2f temp2fNrm = new Vector2f();
private Vector2f temp2fTMP = new Vector2f();
private boolean flip = false;
public void drawLine(float xStart, float yStart, float xEnd, float yEnd){
resetLineStates();
temp2fA.set(xStart, yStart);
temp2fB.set(xEnd, yEnd);
v2fDirection(temp2fA, temp2fB, temp2fDir);
v2fNormal(temp2fDir, temp2fNrm);
float halfThickness = currentLineThickness / 2;
//System.out.println("new line called");
v2fScaleAndAdd(temp2fB, temp2fNrm, -halfThickness, temp2fTMP);
pushVertex(temp2fTMP);
v2fScaleAndAdd(temp2fB, temp2fNrm, halfThickness, temp2fTMP);
pushVertex(temp2fTMP);
v2fScaleAndAdd(temp2fA, temp2fNrm, halfThickness, temp2fTMP);
pushVertex(temp2fTMP);
v2fScaleAndAdd(temp2fA, temp2fNrm, -halfThickness, temp2fTMP);
pushVertex(temp2fTMP);
//System.out.println(indexCount + " before rendering.");
int index = indexCount;
pushIndices(index, index + 1, index + 3);
pushIndices(index + 1, index + 2, index + 3);
//System.out.println(indexCount + " after rendering.");
}
private void resetLineStates(){
temp2fA.set(0);
temp2fB.set(0);
temp2fDir.set(0);
temp2fNrm.set(0);
temp2fTMP.set(0);
}
pushIndices is the following function:
private void pushIndices(int i1, int i2, int i3){
shapeIndices.add(i1);
shapeIndices.add(i2);
shapeIndices.add(i3);
indexCount += 3;
}
And pushVertex works like so:
private void pushVertex(float x, float y, float z){
shapeVertexData[vertexDataOffset] = x;
shapeColourData[vertexDataOffset] = currentShapeColour.x;
shapeVertexData[vertexDataOffset + 1] = y;
shapeColourData[vertexDataOffset + 1] = currentShapeColour.y;
shapeVertexData[vertexDataOffset + 2] = z;
shapeColourData[vertexDataOffset + 2] = currentShapeColour.z;
//System.out.println("\tpushed vertex: " + data.x + ", " + data.y + ", 0");
vertexDataOffset += 3;
}
I'm using the following fields to store vertex data and such - this is all sub-buffered to a VBO when I flush the batch. If the vertex data arrays have not had to grow in size, I will sub-buffer them to their respective VBO, likewise with the element buffer, otherwise if they have had to grow then I re-buffer the VBO to fit.
private float[] shapeVertexData;
private float[] shapeColourData;
private int vertexDataOffset;
private ArrayList<Integer> shapeIndices;
private int indexCount;
When I use my debugger in IDEA, the vertex data appears completely correct in the arrays I'm constructing, but when I explore it in RenderDoc, it's wrong. I don't understand what I'm doing wrong to get these results, and obviously the first two vertices appear completely fine even for the second rectangle, but the others are totally wrong.
I'm confident that my shaders are not the problem, as they're very simple, but here they are:
shape_render.vs (vertex shader):
#version 330
layout (location = 0) in vec3 aPosition;
layout (location = 1) in vec3 aColour;
uniform mat4 modelMatrix;
uniform mat4 viewMatrix;
uniform mat4 projectionMatrix;
flat out vec3 shapeFill;
void main(){
shapeFill = aColour;
gl_Position = projectionMatrix * viewMatrix * modelMatrix * vec4(aPosition.x, aPosition.y, 0.0, 1.0);
}
shape_render.fs (fragment shader):
#version 330
layout (location = 0) out vec4 fragColour;
in vec3 shapeFill;
void main(){
fragColour = vec4(shapeFill, 1);
}
I think I've just about explained it to the best of my knowledge, any insight would be greatly appreciated. I've already checked and determined I'm enabling the necessary vertex arrays, etc... and rendering the correct amount of indices (12):
Thanks so much for having a look at this for me.

I figured it out after thinking about it for a while longer. It was to do with how I was specifying the indices. I was using the correct amount of indices however specifying them incorrectly.
For arguments sake to construct a triangle, the first one would have an index count of 0, and with four vertices, the indices would be 1,2,3 and 2,3,1 (for example). However for each new triangle I was starting the index count at the old count plus six, which makes sense for addressing an array, but because each rectangle only specified four vertices, I was pointing indices at data that didn’t exist.
So instead of using indexCount += 3 each time I pushed indices, I’ll get the current count of vertices instead and build my indices from that.

Rotation won't work in Java physics engine

I am making a java rigid body physics engine, and it has gone great so far, until I tried to implement rotation. I don't know where the problem is coming from. I have methods calculating the moment of inertia of convex polygons and circles using formulas from these websites:
http://lab.polygonal.de/?p=57
http://en.wikipedia.org/wiki/List_of_moments_of_inertia
This is the code for the polygon moment of inertia:
public float momentOfInertia() {
Vector C = centerOfMass().subtract(position); //center of mass
Line[] sides = sides(); //sides of the polygon
float moi = 0; //moment of inertia
for(int i = 0; i < sides.length; i++) {
Line l = sides[i]; //current side of polygon being looped through
Vector p1 = C; //points 1, 2, and 3 are the points of the triangle
Vector p2 = l.point1;
Vector p3 = l.point2;
Vector Cp = p1.add(p2).add(p3).divide(3); //center of mass of the triangle, or C'
float d = new Line(C, Cp).length(); //distance between center of mass
Vector bv = p2.subtract(p1); //vector for side b of triangle
float b = bv.magnitude(); //scalar for length of side b
Vector u = bv.divide(b); //unit vector for side b
Vector cv = p3.subtract(p1); //vector for side c of triangle, only used to calculate variables a and h
float a = cv.dot(u); //length of a in triangle
Vector av = u.multiply(a); //vector for a in triangle
Vector hv = cv.subtract(av); //vector for height of triangle, or h in diagram
float h = hv.magnitude(); //length of height of triangle, or h in diagram
float I = ((b*b*b*h)-(b*b*h*a)+(b*h*a*a)+(b*h*h*h))/36; //calculate moment of inertia of individual triangle
float M = (b*h)/2; //mass or area of triangle
moi += I+M*d*d; //equation in sigma series of website
}
return moi;
}
And this is for the circle:
public float momentOfInertia() {
return (float) Math.pow(radius, 2)*area()/2;
}
I know for a fact that the area functions work fine, I have checked them. I just don't know how to check if the moment of inertia equations are wrong.
For collision detection, I used the separating axis theorem to check for any combination of two polygons and circles, where it can find out whether they are colliding, the normal velocity of the collision, and the contact point of the collision. These methods all work beautifully.
I might also like to say how positions are organized. Every body has a position and a shape, either a polygon or a circle. Each shape has a position, and polygons have individual vertices. So if I want to find the absolute position of a vertex of a polygon-shaped body, I need to add the positions of the body, the polygon, and the vertex itself. The center of mass equation is in absolute position according to the shape, with no account for the body. The center of mass and moment of inertia methods are in the Shape class.
For every body, the constants are being updated according to the force and torque in the body's update method where dt is delta time. I also rotate the polygon based on the difference in rotation, because the vertices are ever changing.
public void update(float dt) {
if(mass != 0) {
momentum = momentum.add(force.multiply(dt));
velocity = momentum.divide(mass);
position = position.add(velocity.multiply(dt));
angularMomentum += torque*dt;
angularVelocity = angularMomentum/momentOfInertia;
angle += angularVelocity*dt;
shape.rotate(angularVelocity*dt);
}
}
Finally, I also have a CollisionResolver class which fixes the collision of two colliding bodies, involving applying the normal force and friction. Here is the class's only method which does all of this:
public static void resolveCollision(Body a, Body b, float dt) {
//calculate normal vector
Vector norm = CollisionDetector.normal(a, b);
Vector normb = norm.multiply(-1);
//undo overlap between bodies
float ratio1 = a.mass/(a.mass+b.mass);
float ratio2 = b.mass/(b.mass+a.mass);
a.position = a.position.add(norm.multiply(ratio1));
b.position = b.position.add(normb.multiply(ratio2));
//calculate contact point of collision and other values needed for rotation
Vector cp = CollisionDetector.contactPoint(a, b, norm);
Vector c = a.shape.centerOfMass().add(a.position);
Vector cb = b.shape.centerOfMass().add(b.position);
Vector d = cp.subtract(c);
Vector db = cp.subtract(cb);
//create the normal force vector from the velocity
Vector u = norm.unit();
Vector ub = u.multiply(-1);
Vector F = new Vector(0, 0);
boolean doA = a.mass != 0;
if(doA) {
F = a.force;
}else {
F = b.force;
}
Vector n = new Vector(0, 0);
Vector nb = new Vector(0, 0);
if(doA) {
Vector Fyp = u.multiply(F.dot(u));
n = Fyp.multiply(-1);
nb = Fyp;
}else{
Vector Fypb = ub.multiply(F.dot(ub));
n = Fypb;
nb = Fypb.multiply(-1);
}
//calculate normal force for body A
float r = a.restitution;
Vector v1 = a.velocity;
Vector vy1p = u.multiply(u.dot(v1));
Vector vx1p = v1.subtract(vy1p);
Vector vy2p = vy1p.multiply(-r);
Vector v2 = vy2p.add(vx1p);
//calculate normal force for body B
float rb = b.restitution;
Vector v1b = b.velocity;
Vector vy1pb = ub.multiply(ub.dot(v1b));
Vector vx1pb = v1b.subtract(vy1pb);
Vector vy2pb = vy1pb.multiply(-rb);
Vector v2b = vy2pb.add(vx1pb);
//calculate friction for body A
float mk = (a.friction+b.friction)/2;
Vector v = a.velocity;
Vector vyp = u.multiply(v.dot(u));
Vector vxp = v.subtract(vyp);
float fk = -n.multiply(mk).magnitude();
Vector fkv = vxp.unit().multiply(fk); //friction force
Vector vr = vxp.subtract(d.multiply(a.angularVelocity));
Vector fkvr = vr.unit().multiply(fk); //friction torque - indicated by r for rotation
//calculate friction for body B
Vector vb = b.velocity;
Vector vypb = ub.multiply(vb.dot(ub));
Vector vxpb = vb.subtract(vypb);
float fkb = -nb.multiply(mk).magnitude();
Vector fkvb = vxpb.unit().multiply(fkb); //friction force
Vector vrb = vxpb.subtract(db.multiply(b.angularVelocity));
Vector fkvrb = vrb.unit().multiply(fkb); //friction torque - indicated by r for rotation
//move bodies based on calculations
a.momentum = v2.multiply(a.mass).add(fkv.multiply(dt));
if(a.mass != 0) {
a.velocity = a.momentum.divide(a.mass);
a.position = a.position.add(a.velocity.multiply(dt));
}
b.momentum = v2b.multiply(b.mass).add(fkvb.multiply(dt));
if(b.mass != 0) {
b.velocity = b.momentum.divide(b.mass);
b.position = b.position.add(b.velocity.multiply(dt));
}
//apply torque to bodies
float t = (d.cross(fkvr)+d.cross(n));
float tb = (db.cross(fkvrb)+db.cross(nb));
if(a.mass != 0) {
a.angularMomentum = t*dt;
a.angularVelocity = a.angularMomentum/a.momentOfInertia;
a.angle += a.angularVelocity*dt;
a.shape.rotate(a.angularVelocity*dt);
}
if(b.mass != 0) {
b.angularMomentum = tb*dt;
b.angularVelocity = b.angularMomentum/b.momentOfInertia;
b.angle += b.angularVelocity*dt;
b.shape.rotate(b.angularVelocity*dt);
}
}
As for the actual problem, both the circles and polygons rotate very slowly and often in wrong directions. I know I am throwing a lot out there, but this problem has been bugging me for a while, and I would appreciate any help I can get.
Thanks.

This answer addresses the "I just don't know how to check if the moment of inertia equations are wrong." part of the question.
There are several possible approaches, some of which you may have already tried, and they can be used in combination:
Unit testing
Take your moment of inertia code and apply it to problems with known solutions from a tutorial or textbook.
Dimensional analysis
I would recommend this anyway for any scientific or engineering program. You may have deleted comments for compactness of posted code, but they are important. Annotate each variable that represents a physical quantity with its units. Check that every expression you evaluate has the right units, based on its inputs, for its result variable. For example, in the classic equation F=ma in SI units: F is in Newtons, equivalent to kg.m/(s^2), m is in kg, a is in m/(s^2), so it all balances. Be careful with transitions between physics world coordinates and screen coordinates.
Program simplification
Try working first with only one instance of one very simple shape for which you can do all the calculations by hand. Since some of your problems do not relate to rotation, a circle may be a good first choice because of its symmetry. Debug that, comparing intermediate results to equivalent results from paper-and-pencil (and calculator). Gradually add more instances of the same shape, then debug a single instance of the next shape...
Deliberate error
Given that you suspect your inertia calculations, try setting arbitrary values slightly different from your calculations, and see what differences they make in the display. Are the effects similar to the problems you are seeing? If so, keep it as a hypothesis.
As a more general note, programs that do iterative simulation can be very vulnerable to accumulated floating point error. Unless you have a real need to save space, and have done enough analysis of the numerical stability of your code to be sure float is OK, I strongly recommend using double instead. This is probably not your current problem, but is something that could become an issue later.

safeguard against floating point errors in java

As the number of frames rendered in a game increase beyond a certain
limit, instead of obtaining an identity matrix for the following type of matrix transformation:
Matrix.setIdentity(ModelMatrix);
Matrix.translate(ModelMatrix, xmov,ymov,0);
Matrix.translate(ModelMatrix, -xmov,-ymov,0);
there are small values that get added to the columns (because of floating point errors in java), and progressively become larger in the matrix (which is no longer identity) and causes strange translations. Below is the code:
...// _ModelMatrixNozzle is set as identity matrix like all other 4x4 matrices in my app in onSurfaceChanged method
...// this code is part of update() method, called by onDrawFrame() in renderer thread
GLES20Renderer._uNozzleCentreMatrix[0] = GLES20Renderer._ModelMatrixNozzle[0] * GLES20Renderer._uNozzleCentre[0] + GLES20Renderer._ModelMatrixNozzle[4] * GLES20Renderer._uNozzleCentre[1] + GLES20Renderer._ModelMatrixNozzle[8] * GLES20Renderer._uNozzleCentre[2] + GLES20Renderer._ModelMatrixNozzle[12] * GLES20Renderer._uNozzleCentre[3];
GLES20Renderer._uNozzleCentreMatrix[1] = GLES20Renderer._ModelMatrixNozzle[1] * GLES20Renderer._uNozzleCentre[0] + GLES20Renderer._ModelMatrixNozzle[5] * GLES20Renderer._uNozzleCentre[1] + GLES20Renderer._ModelMatrixNozzle[9] * GLES20Renderer._uNozzleCentre[2] + GLES20Renderer._ModelMatrixNozzle[13] * GLES20Renderer._uNozzleCentre[3];
GLES20Renderer._uNozzleCentreMatrix[2] = 0;
GLES20Renderer._uNozzleCentreMatrix[3] = 1;
if(Math.abs(ds) > 0) {
/*transformations will be added here if the errors are solved*/
} else {
if(GLES20Renderer._zAngle >= 360) {
GLES20Renderer._zAngle = GLES20Renderer._zAngle - 360;
}
if(GLES20Renderer._zAngle <= -360) {
GLES20Renderer._zAngle = GLES20Renderer._zAngle + 360;
}
Matrix.translateM(GLES20Renderer._ModelMatrixNozzle, 0, GLES20Renderer._uNozzleCentreMatrix[0], GLES20Renderer._uNozzleCentreMatrix[1], 0);
Matrix.rotateM(GLES20Renderer._ModelMatrixNozzle, 0, GLES20Renderer._zAngle, 0, 0, 1);
//Matrix.rotateM(GLES20Renderer._ModelMatrixNozzle, 0, -GLES20Renderer._lastZAngle, 0, 0, 1);
Matrix.translateM(GLES20Renderer._ModelMatrixNozzle, 0, -GLES20Renderer._uNozzleCentreMatrix[0], -GLES20Renderer._uNozzleCentreMatrix[1], 0);
}
Download apk:http://www.pixdip.com/opengles/rotation/floating.apk
Although this is not required, but the complete code is here:Drift in rotation about z-axis

Matrix.setIdentity(ModelMatrix);
Matrix.translate(ModelMatrix, xmov,ymov,0);
Matrix.translate(ModelMatrix, -xmov,-ymov,0);
Will always produce floating point errors because of matrix stack.
The way I finally removed this was by using seperate matrices for such critical transformations:
private static float[] _TMatrix = new float[16];
private static float[] _ModelMatrix = new float[16];
Matrix.setIdentity(Renderer._ModelMatrix);
Matrix.setIdentity(Renderer._TMatrix);
Matrix.translate(Renderer._ModelMatrix, xmov,ymov,0);
Matrix.translate(Renderer._TMatrix, -xmov,-ymov,0);
Matrix.multiply(Renderer._ModelMatrix, Renderer._TMatrix, Renderer._ModelMatrix);
// will result in an identity model matrix, without any floating point errors

Virtual trackball implementation

I've been trying for the past few days to make a working implementation of a virtual trackball for the user interface for a 3D graphing-like program. But I'm having trouble.
Looking at the numbers and many tests the problems seems to be the actual concatenation of my quaternions but I don't know or think so. I've never worked with quaternions or virtual trackballs before, this is all new to me. I'm using the Quaternion class supplied by JOGL. I tried making my own and it worked (or at least as far a I know) but it was a complete mess so I just went with JOGL's.
When I do not concatenate the quaternions the slight rotations I see seem to be what I want, but of course It's hard when it's only moving a little bit in any direction. This code is based off of the Trackball Tutorial on the OpenGL wiki.
When I use the Quaternion class's mult (Quaternion q) method the graph hardly moves (even less than not trying to concatenate the quaternions).
When I tried Quaternionclass'sadd (Quaternion q)` method for the fun of it I get something that at the very least rotates the graph but not in any coherent way. It spazzes out and rotates randomly as I move the mouse. Occasionally I'll get quaternions entirely filled with NaN.
In my code I will not show either of these, I'm lost with what to do with my quaternions. I know I want to multiply them because as far as I'm aware that's how they are concatenated. But like I said I've had no success, I'm assuming the screw up is somewhere else in my code.
Anyway, my setup has a Trackball class with a public Point3f projectMouse (int x, int y) method and a public void rotateFor (Point3f p1, Point3f p2), Where Point3f is a class I made. Another class called Camera has a public void transform (GLAutoDrawable g) method which will call OpenGL methods to rotate based on the trackball's quaternion.
Here's the code:
public Point3f projectMouse (int x, int y)
{
int off = Screen.WIDTH / 2; // Half the width of the GLCanvas
x = x - objx_ - off; // obj being the 2D center of the graph
y = off - objy_ - y;
float t = Util.sq(x) + Util.sq(y); // Util is a class I made with
float rsq = Util.sq(off); // simple some math stuff
// off is also the radius of the sphere
float z;
if (t >= rsq)
z = (rsq / 2.0F) / Util.sqrt(t);
else
z = Util.sqrt(rsq - t);
Point3f result = new Point3f (x, y, z);
return result;
}
Here's the rotation method:
public void rotateFor (Point3f p1, Point3f p2)
{
// Vector3f is a class I made, I already know it works
// all methods in Vector3f modify the object's numbers
// and return the new modify instance of itself
Vector3f v1 = new Vector3f(p1.x, p1.y, p1.z).normalize();
Vector3f v2 = new Vector3f(p2.x, p2.y, p2.z).normalize();
Vector3f n = v1.copy().cross(v2);
float theta = (float) Math.acos(v1.dot(v2));
float real = (float) Math.cos(theta / 2.0F);
n.multiply((float) Math.sin(theta / 2.0F));
Quaternion q = new Quaternion(real, n.x, n.y, n.z);
rotation = q; // A member that can be accessed by a getter
// Do magic on the quaternion
}
EDIT:
I'm getting closer, I found out a few simple mistakes.
1: The JOGL implementation treats W as the real number, not X, I was using X for real
2: I was not starting with the quaternion 1 + 0i + 0j + 0k
3: I was not converting the quaternion into an axis/angle for opengl
4: I was not converting the angle into degrees for opengl
Also as Markus pointed out I was not normalizing the normal, when I did I couldn't see much change, thought it's hard to tell, he's right though.
The problem now is when I do the whole thing the graph shakes with a fierceness like you would never believe. It (kinda) moves in the direction you want it to, but the seizures are too fierce to make anything out of it.
Here's my new code with a few name changes:
public void rotate (Vector3f v1, Vector3f v2)
{
Vector3f v1p = v1.copy().normalize();
Vector3f v2p = v2.copy().normalize();
Vector3f n = v1p.copy().cross(v2p);
if (n.length() == 0) return; // Sometimes v1p equals v2p
float w = (float) Math.acos(v1p.dot(v2p));
n.normalize().multiply((float) Math.sin(w / 2.0F));
w = (float) Math.cos(w / 2.0F);
Quaternion q = new Quaternion(n.x, n.y, n.z, w);
q.mult(rot);
rot_ = q;
}
Here's the OpenGL code:
Vector3f p1 = tb_.project(x1, y1); // projectMouse [changed name]
Vector3f p2 = tb_.project(x2, y2);
tb_.rotate (p1, p2);
float[] q = tb_.getRotation().toAxis(); // Converts to angle/axis
gl.glRotatef((float)Math.toDegrees(q[0]), q[1], q[2], q[3]);
The reason for the name changes is because I deleted everything in the Trackball class and started over. Probably not the greatest idea, but oh well.
EDIT2:
I can say with pretty good certainty that there is nothing wrong with projecting onto the sphere.
I can also say that as far as the whole thing goes it seems to be the VECTOR that is the problem. The angle looks just fine, but the vector seems to jump around.
EDIT3:
The problem is the multiplication of the two quaternions, I can confirm that everything else works as expected. Something goes whacky with the axis during multiplication!

The problem is the multiplication of the two quaternions, I can confirm that everything else works as expected. Something goes whacky with the axis during multiplication!
You are absolutely correct!! I just recently submitted a correct multiplication and Jogamp has accepted my change. They had incorrect multiplication on mult(quaternion).
I am sure if you get the latest jogl release, it'll have the correct mult(Quaternion)

I did it!
Thanks to this C++ implementation I was able to develop a working trackball/arcball interface. My goodness me, I'm still not certain what the problem was, but I rewrote everything and even wrote my own Quaternions class and suddenly the whole thing works. I also made a Vectors class for vectors. I had a Vector3f class before but the Quaternions and Vectors classes are full of static methods and take in arrays. To make it easy to do vector computations on quaternions and vice versa. I will link the code for those two classes below, but only the Trackball class will be show here.
I made those two classes pretty quickly this morning so if there are any mathematical errors, well, uh, oops. I only used what I needed to use and made sure they were correct. These classes are below:
Quaternions: http://pastebin.com/raxS4Ma9
Vectors: http://pastebin.com/fU3PKZB9
Here is my Trackball class:
public class Trackball
{
private static final float RADIUS_ = Screen.DFLT_WIDTH / 2.0F;
private static final int REFRESH_ = 50;
private static final float SQRT2_ = (float) Math.sqrt(2);
private static final float SQRT2_INVERSE_ = 1.0F / SQRT2_;
private int count_;
private int objx_, objy_;
private float[] v1_, v2_;
private float[] rot_;
public Trackball ()
{
v1_ = new float[4];
v2_ = new float[4];
rot_ = new float[] {0, 0, 0, 1};
}
public void click (int x, int y)
{
v1_ = project(x, y);
}
public void drag (int x, int y)
{
v2_ = project(x, y);
if (Arrays.equals(v1_, v2_)) return;
float[] n = Vectors.cross(v2_, v1_, null);
float[] o = Vectors.sub(v1_, v2_, null);
float dt = Vectors.len(o) / (2.0F * RADIUS_);
dt = dt > 1.0F ? 1.0F : dt < -1.0F ? -1.0F : dt;
float a = 2.0F * (float) Math.asin(dt);
Vectors.norm_r(n);
Vectors.mul_r(n, (float) Math.sin(a / 2.0F));
if (count_++ == REFRESH_) { count_ = 0; Quaternions.norm_r(rot_); }
float[] q = Arrays.copyOf(n, 4);
q[3] = (float) Math.cos(a / 2.0F);
rot_ = Quaternions.mul(q, rot_, rot_);
}
public float[] getAxis ()
{
return Quaternions.axis(rot_, null);
}
public float[] project (float x, float y)
{
x = RADIUS_ - objx_ - x;
y = y - objy_ - RADIUS_;
float[] v = new float[] {x, y, 0, 0};
float len = Vectors.len(v);
float tr = RADIUS_ * SQRT2_INVERSE_;
if (len < tr)
v[2] = (float) Math.sqrt(RADIUS_ * RADIUS_ - len * len);
else
v[2] = tr * tr / len;
return v;
}
}
You can see there's a lot of similarities from the C++ example. Also I'd like to note there is no method for setting the objx_ and objy_ values yet. Those are for setting the center of the graph which can be moved around. Just saying, so you don't scratch your head about those fields.

The cross-product of two normalized vectors is not normalized itself. It's length is sin(theta). Try this instead:
n = n.normalize().multiply((float) Math.sin(theta / 2.0F));