Develop Reference

3D - Rotating point around vector using quaternions

So i have a point (x,y,z) and a vector (x1,y1,z1) and i want to rotate the point around the vector in a 3D space. From what i read I should be able to do so using quaternions like this:
(0,new_x,new_y,new_z) = K^-1 * I * K
where K = (cos(fi/2), sin(fi/2)*(x1,y1,z1)) (where (x1,y1,z1) is a normalized vector)
I = (0,(x,y,z))
K^-1 = (cos(fi/2), -sin(fi/2)*(x1,y1,z1))
I implemented it like this:
Point3D n = new Point3D(x1,y1,z1);
n=n.normalize();
double a=Math.cos(Math.toRadians(45)); //fi is 90
double b= Math.sin(Math.toRadians(45));
double k_a = a;
double k_b = b*n.getX();
double k_c=b*n.getY();
double k_d = b*n.getZ(); //K points
double k_a2=k_a; //K^-1 points
double k_b2=-k_b;
double k_c2 = -k_c;
double k_d2= -k_d;
//I*K
double a_m = -((x*k_b)+(y*k_c)+(z*k_d));
double b_m= k_a*x+y*k_d+0*k_b-z*k_c;
double c_m = k_a*y+0*k_c+k_b*z-x*k_d;
double d_m = k_a*z+0*k_d+x*k_c-y*k_b;
//K^-1 * what we got above aka the final coordinates
double a_f = k_a2*a_m -b_m*k_b2-c_m*k_c2-d_m*k_d2; //should and is 0
double x_f= k_a2*b_m+a_m*k_b2+k_c2*d_m-k_d2*c_m;
double y_f = k_a2*c_m+a_m*k_c2+k_b2*d_m-k_d2*b_m;
double z_f = k_a2*d_m+a_m*k_d2+k_b2*c_m-k_c2*b_m;
The problem is, that when i use the above code for a animation (rotating a sphere around a vector), instead of a circle i get a spiral, where the sphere quickly ends up in the same place as the vector:
The move it self is done with a button click for now like this:
btn2.setOnAction(new EventHandler() {
#Override
public void handle(ActionEvent e) {
Point3D n = calc(x,y,z,x1,y1,z1); //a call to the method calculating K^-1*I*K shown above
Sphere sphere= new Sphere(10); //I know, drawing a new one everytime is a waste, but i wanted to be sure the translate wasnt at fault since im new at javaFX
sphere.setMaterial(new PhongMaterial(Color.CORAL));
sphere.setTranslateX(n.getX());
sphere.setTranslateY(n.getY());
sphere.setTranslateZ(n.getZ());
x=n.getX();
y=n.getY();
z=n.getZ();
content.group.getChildren().remove(0);
content.group.getChildren().add(0, sphere);
}
});
I think the problem is in the calculation of the new coordinates, after a bit they end up somewhere on the vector, but after rechecking the math more times than i can count, im officially lost. Can anyone tell me what i am missing or where i went wrong?

Oh nevermind, it was an calculation mistake afterall (though i swear i checked it over a thousand times..)
instead of:
double y_f = k_a2*c_m+a_m*k_c2+k_b2*d_m-k_d2*b_m;
its supposed to be:
double y_f = k_a2*c_m+a_m*k_c2-k_b2*d_m+k_d2*b_m;

Issues with Raytracing triangles (orientation and coloring)

EDIT: I found out that all the pixels were upside down because of the difference between screen and world coordinates, so that is no longer a problem.
EDIT: After following a suggestion from #TheVee (using absolute values), my image got much better, but I'm still seeing issues with color.
I having a little trouble with ray-tracing triangles. This is a follow-up to my previous question about the same topic. The answers to that question made me realize that I needed to take a different approach. The new approach I took worked much better, but I'm seeing a couple of issues with my raytracer now:
There is one triangle that never renders in color (it is always black, even though it's color is supposed to be yellow).
Here is what I am expecting to see:
But here is what I am actually seeing:
Addressing debugging the first problem, even if I remove all other objects (including the blue triangle), the yellow triangle is always rendered black, so I don't believe that it is an issues with my shadow rays that I am sending out. I suspect that it has to do with the angle that the triangle/plane is at relative to the camera.
Here is my process for ray-tracing triangles which is based off of the process in this website.
Determine if the ray intersects the plane.
If it does, determine if the ray intersects inside of the triangle (using parametric coordinates).
Here is the code for determining if the ray hits the plane:
private Vector getPlaneIntersectionVector(Ray ray)
{
double epsilon = 0.00000001;
Vector w0 = ray.getOrigin().subtract(getB());
double numerator = -(getPlaneNormal().dotProduct(w0));
double denominator = getPlaneNormal().dotProduct(ray.getDirection());
//ray is parallel to triangle plane
if (Math.abs(denominator) < epsilon)
{
//ray lies in triangle plane
if (numerator == 0)
{
return null;
}
//ray is disjoint from plane
else
{
return null;
}
}
double intersectionDistance = numerator / denominator;
//intersectionDistance < 0 means the "intersection" is behind the ray (pointing away from plane), so not a real intersection
return (intersectionDistance >= 0) ? ray.getLocationWithMagnitude(intersectionDistance) : null;
}
And once I have determined that the ray intersects the plane, here is the code to determine if the ray is inside the triangle:
private boolean isIntersectionVectorInsideTriangle(Vector planeIntersectionVector)
{
//Get edges of triangle
Vector u = getU();
Vector v = getV();
//Pre-compute unique five dot-products
double uu = u.dotProduct(u);
double uv = u.dotProduct(v);
double vv = v.dotProduct(v);
Vector w = planeIntersectionVector.subtract(getB());
double wu = w.dotProduct(u);
double wv = w.dotProduct(v);
double denominator = (uv * uv) - (uu * vv);
//get and test parametric coordinates
double s = ((uv * wv) - (vv * wu)) / denominator;
if (s < 0 || s > 1)
{
return false;
}
double t = ((uv * wu) - (uu * wv)) / denominator;
if (t < 0 || (s + t) > 1)
{
return false;
}
return true;
}
Is think that I am having some issue with my coloring. I think that it has to do with the normals of the various triangles. Here is the equation I am considering when I am building my lighting model for spheres and triangles:
Now, here is the code that does this:
public Color calculateIlluminationModel(Vector normal, boolean isInShadow, Scene scene, Ray ray, Vector intersectionPoint)
{
//c = cr * ca + cr * cl * max(0, n \dot l)) + cl * cp * max(0, e \dot r)^p
Vector lightSourceColor = getColorVector(scene.getLightColor()); //cl
Vector diffuseReflectanceColor = getColorVector(getMaterialColor()); //cr
Vector ambientColor = getColorVector(scene.getAmbientLightColor()); //ca
Vector specularHighlightColor = getColorVector(getSpecularHighlight()); //cp
Vector directionToLight = scene.getDirectionToLight().normalize(); //l
double angleBetweenLightAndNormal = directionToLight.dotProduct(normal);
Vector reflectionVector = normal.multiply(2).multiply(angleBetweenLightAndNormal).subtract(directionToLight).normalize(); //r
double visibilityTerm = isInShadow ? 0 : 1;
Vector ambientTerm = diffuseReflectanceColor.multiply(ambientColor);
double lambertianComponent = Math.max(0, angleBetweenLightAndNormal);
Vector diffuseTerm = diffuseReflectanceColor.multiply(lightSourceColor).multiply(lambertianComponent).multiply(visibilityTerm);
double angleBetweenEyeAndReflection = scene.getLookFrom().dotProduct(reflectionVector);
angleBetweenEyeAndReflection = Math.max(0, angleBetweenEyeAndReflection);
double phongComponent = Math.pow(angleBetweenEyeAndReflection, getPhongConstant());
Vector phongTerm = lightSourceColor.multiply(specularHighlightColor).multiply(phongComponent).multiply(visibilityTerm);
return getVectorColor(ambientTerm.add(diffuseTerm).add(phongTerm));
}
I am seeing that the dot product between the normal and the light source is -1 for the yellow triangle, and about -.707 for the blue triangle, so I'm not sure if the normal being the wrong way is the problem. Regardless, when I added made sure the angle between the light and the normal was positive (Math.abs(directionToLight.dotProduct(normal));), it caused the opposite problem:
I suspect that it will be a small typo/bug, but I need another pair of eyes to spot what I couldn't.
Note: My triangles have vertices(a,b,c), and the edges (u,v) are computed using a-b and c-b respectively (also, those are used for calculating the plane/triangle normal). A Vector is made up of an (x,y,z) point, and a Ray is made up of a origin Vector and a normalized direction Vector.
Here is how I am calculating normals for all triangles:
private Vector getPlaneNormal()
{
Vector v1 = getU();
Vector v2 = getV();
return v1.crossProduct(v2).normalize();
}
Please let me know if I left out anything that you think is important for solving these issues.
EDIT: After help from #TheVee, this is what I have at then end:
There are still problems with z-buffering, And with phong highlights with the triangles, but the problem I was trying to solve here was fixed.

It is an usual problem in ray tracing of scenes including planar objects that we hit them from a wrong side. The formulas containing the dot product are presented with an inherent assumption that light is incident at the object from a direction to which the outer-facing normal is pointing. This can be true only for half the possible orientations of your triangle and you've been in bad luck to orient it with its normal facing away from the light.
Technically speaking, in a physical world your triangle would not have zero volume. It's composed of some layer of material which is just thin. On either side it has a proper normal that points outside. Assigning a single normal is a simplification that's fair to take because the two only differ in sign.
However, if we made a simplification we need to account for it. Having what technically is an inwards facing normal in our formulas gives negative dot products, which case they are not made for. It's like light was coming from the inside of the object or that it hit a surface could not possibly be in its way. That's why they give an erroneous result. The negative value will subtract light from other sources, and depending on the magnitude and implementation may result in darkening, full black, or numerical underflow.
But because we know the correct normal is either what we're using or its negative, we can simply fix the cases at once by taking a preventive absolute value where a positive dot product is implicitly assumed (in your code, that's angleBetweenLightAndNormal). Some libraries like OpenGL do that for you, and on top use the additional information (the sign) to choose between two different materials (front and back) you may provide if desired. Alternatively, they can be set to not draw the back faces for solid object at all because they will be overdrawn by front faces in solid objects anyway (known as face culling), saving about half of the numerical work.

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.

"Is Triangle Touching" code not giving correct results

The code I'm using in my collision detection code is this:
(note: Vector3f is part of the LWJGL library.)
(Note2:Tri is a class composed of three of LWJGL's Vector3fs. v1, v2, and v3.)
public Vector<Tri> getTrisTouching(Vector3f pos, float radius){
Vector<Tri> tempVec = new Vector<Tri>();
for(int i = 0; i < tris.size(); i++){
Tri t = tris.get(i);
Vector3f one_to_point = new Vector3f(0,0,0);
Vector3f.sub(pos,t.v1,one_to_point); //Storing vector A->P
Vector3f one_to_two = new Vector3f(0,0,0);
Vector3f.sub(t.v2,t.v1, one_to_two); //Storing vector A->B
Vector3f one_to_three = new Vector3f(0,0,0);
Vector3f.sub(t.v3, t.v1, one_to_three); //Storing vector A->C
float q1 = Vector3f.dot(one_to_point, one_to_two) / one_to_two.lengthSquared(); // The normalized "distance" from a to
float q2 = Vector3f.dot(one_to_point, one_to_three) / one_to_three.lengthSquared(); // The normalized "distance" from a to
if (q1 > 0 && q2 > 0 && q1 + q2 < 1){
tempVec.add(t);
}
}
return tempVec;
}
My question is how do I correctly see if a point in space is touching one of my triangles?

To test if your point is inside the triangle, create a ray with its origin at the test point and extend it to infinity. A nice easy one would be a ray which is horizontal ( e.g. y constant, and x increases to infinity. Then count the number of times it intersects with one of your polygon edges. Zero or an even number of intersections means you are outside the triangle. The nice thing about this it works not just for triangles but any polygon.
http://erich.realtimerendering.com/ptinpoly/

The only way I can help you is by providing you with this link.
Unfortunately, I'm not very good with LWJGL Geometry so here you are. - Vector3f
Hope it helps! If it does, please tick the answer to accept.

Line detection | Angle detection with Java

I'm processing some images that my UGV (Unmanned Ground Vehichle) captures to make it move on a line.
I want to get the angle of that line based on the horizon. I'll try to explain with a few examples:
The image above would make my UGV to keep straight ahead, as the angle is about 90 degrees.
But the following would make it turn left, as the angle compaired to the horizon rounds about 120.
I could successfully transform those images into the image below using otsu for thresholding:
And also used an edge detection algorithm to get this:
But I'm stuck right now trying to find an algorithm that detecs those edges/lines and outputs - or helps me to output - the angle of such line..

Here's my attempt using ImageJ:
// Open the Image
ImagePlus image = new ImagePlus(filename);
// Make the Image 8 bit
IJ.run(image, "8-bit", "");
// Apply a Threshold (0 - 50)
ByteProcessor tempBP = (ByteProcessor)image.getProcessor();
tempBP.setThreshold(0, 50, 0);
IJ.run(image, "Convert to Mask", "");
// Analyze the Particles
ParticleAnalyzer pa = new ParticleAnalyzer(
ParticleAnalyzer.SHOW_MASKS +
ParticleAnalyzer.IN_SITU_SHOW,
1023 +
ParticleAnalyzer.ELLIPSE
, rt, 0.0, 999999999, 0, 0.5);
IJ.run(image, "Set Measurements...", "bounding fit redirect=None decimal=3");
pa.analyze(image);
int k = 0;
double maxSize = -1;
for (int i = 0; i < rt.getCounter(); i ++) {
// Determine creteria for best oval.
// The major axis should be much longer than the minor axis.
// let k = best oval
}
double bx = rt.getValue("BX", k);
double by = rt.getValue("BY", k);
double width = rt.getValue("Width", k);
double height = rt.getValue("Height", k);
// Your angle:
double angle = rt.getValue("Angle", k);
double majorAxis = rt.getValue("Major", k);
double minorAxis = rt.getValue("Minor", k);
How the code works:
Make the image grayscaled.
Apply a threshold on it to only get the dark areas. This assumes the lines will always be near black.
Apply a Particle Analyzer to find Ellipses on the image.
Loop through the "Particles" to find ones that fit our criteria.
Get the angle from our Particle.
Here's an example of what the image looks like when I analyze it:
NOTE: The code is untested. I just converted what I did in the Visual ImageJ into Java.