Floating-point error when checking for coplanar 3D points - c++

I am looking for an algorithm to check if a point is coplanar with a given 3D plane, defined out of three vertices, while minimizing floating point errors.
I would like to minimize the amount of multiplications and division to mitigate floating point errors.
My implementation uses floats, I cannot go double.
I cannot use an external library.
My current method suffers from these errors:
I have code defining a plane using the general form of the plane equation:
ax + by + cz + d = 0
I compute these coefficients using three 3D vertices v0, v1 and v2 as follow:
// Pseudo-code to define a plane (with class Vector3 defining a vector in 3D)
Vector3 A = v1 - v0;
Vector3 B = v2 - v0;
Vector3 N = cross_product(A,B); // Normal vector
N.Normalize(); // Unit normal vector storing coefs. a, b, c
float d = dot_product(N,v0);
To check if another vertex p is coplanar, I plug the point into the plane equation and check if the result is 0:
// Pseudo-code for coplanar test:
bool is_coplanar()
{
float res = N.x()*p.x() + N.y()*p.y() + N.z()*p.z() - d;
return true if res is "almost" null; // "almost" is: abs(res)<EPSILON
}
My code fails in this case:
v0 = [-8.50001907, 0, 323]
v1 = [8.49998093, 0, 323]
v2 = [-8.50001907, 1.49999976, 322.598083]
Then the plane coefficients are:
N = [-0, 0.258814692, 0.965926945]
d = 311.994415
And when I plug the point v2, I find a result "far" from 0 (although v2 was used to define the plane):
res = -3.05175781e-05
My EPSILON is currently 1e-5.
Tested on compiler qcc 4.4.2 (QNX Momentics, similar to gcc). With no optimization -O0.

Such geometric predicates suffer in a lot of ways from floating point errors. The only industrial strength solution is to use adaptable arithmetic filtering (provided that a robust implementation of the coplanar test is not covering you).
Luckily such implementations (that would take quite some time to write) are already available. In the previous link the orient3d predicate does what you need: Given 3 plane forming points, decide whether a 4th one lies above,below or on the plane
If such an implementation is an overkill, check the simple one. It offers 4 in total:
orient3dfast() Approximate 3D orientation test. Nonrobust.
orient3dexact() Exact 3D orientation test. Robust.
orient3dslow() Another exact 3D orientation test. Robust.
orient3d() Adaptive exact 3D orientation test. Robust.
Disclaimer: The code listing is provided as a tutorial of the mathematical concepts and programming techniques needed to reach a robust solution. I'm neither suggesting nor implying copy-pasting anything.

Related

Is it logical wanting to combine CGAL with Quaternions

I am creating a Discrete Element Method simulation program and I am using CGAL to describe the polyhedrons. From reading literature I was planning to do my differential equations for rotation with Quaternions due to the better numerical stability and lack of gimbal lock. However CGAL does not seem to support rotation based on quaternions. (Please tell me if I am incorrect here) I find it a bit surprising that this seems to be missing, certainly since CGAL likes to be absolute in its accuracy which seems to fit well with the numerical stability of quaternions.
Question: Can I somehow combine Boost Quaternions with CGAL or is there any easy way to implement this. And if so, would this be a logical idea to try?
The other options I think I have are:
writing my differential equations for the affine rotation used is CGAL and deal with the downsides there.
store the orientation as an affine rotation matrix and convert it to Quaternions and use this in the diff. equations. Obviously I am worried about the needed conversion step here every timestep.
Any suggestions or other options that I might think of are greatly appreciated.
First Option: Use the Aff_transformation_3 class
CGAL does not provide a quaternion class, it does provide the Aff_transformation_3 class though. Which you could easily use like this:
CGAL::Surface_mesh<Kernel> P;
std::transform( P.points_begin(), P.points_end(), P.points_begin(), yourAffineTransformation);
for defining the transformation matrix see this.
Second Option: Use Quaternions
If you want to use quaternions you would need to construct one with an external library. For example you could use Eigen:
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h> //or whichever kernel suits your needs
#include <CGAL/Surface_mesh.h>
#include <Eigen/Geometry>
using Kernel = CGAL::Exact_predicates_inexact_constructions_kernel;
using Polyhedron = CGAL::Surface_mesh<Kernel>;
using Point = CGAL::Point_3<Kernel>;
// define the function that rotates your mesh
template <typename Vect, typename Quaternion>
void rotateCGALPolyhedron(Polyhedron P, Vect to_rotation_center,
Quaternion quat) {
for (auto vi : P.vertices()) {
Point p = P.point(vi);
// translate your point to the rotation center. In your case this would be
// the center of mass of the Polyhderon
Vect V(p[0] - to_rotation_center[0], p[1] - to_rotation_center[1],
p[2] - to_rotation_center[2]);
// construct the translation vector that moves your point to the rotated
// position
Vect v = quat * V; //the Vect operator*(Quaternion, Vect) must be implemented!! If you use Eigen::Quaternion you could use Eigen::Vector3d
// retranslate the point back to its initial position and translate it using
// the previously created translation vector
P.point(size_t(vi)) =
Point(to_rotation_center[0] + v[0], to_rotation_center[1] + v[1],
to_rotation_center[2] + v[2]);
}
}
int main() {
// define your rotation using eigen's quaternion class
Eigen::Quaternion<double> quad(..);
Eigen::Vector_3d centerOfMass; //find the center of mass of the mesh you want to rotate
rotateCGALPolyhedron(P.vertices.begin(), P.vertices.end(), centerOfMass,
quad);
return 0;
}
As you can see since cgal does not have an implementation for quaternions if you want to use quaternions the code is lengthy compared to the Aff_transformation_3 case.

How can this code retrieve a 2D vector from a cross-product of two 2D vectors?

I am in a lost. I have been trying to implement this code at:http://www.blackpawn.com/texts/pointinpoly/default.html
However, I don't know how is it possible that the cross-product present there between two 2D vectors can result also in a 2D vector. It does not make sense to me. That is also present in some examples of intersection between polygons and lines, in the fine book "Realtime Collision Detection" - where even scalar triples between 2D vectors appear in the codes (see page 189, for instance).
The issue is that, as far as I can think of it, the pseudo cross-product of two 2D vectors can only result in a scalar (v1.xv2.y-v1.yv2.x) or at most in a 3D vector if one adds two zeros, since that scalar represents the Z dimension. But how can it result in a 2D vector?
I am not the first one to ask this and, coincidently, when trying to use the same code example: Cross product of 2 2D vectors However, as can be easily seen, the answer, the original question when updated and the comments in that thread ended up being quite a mess, if I dare say so.
Does anyone know how should I get these 2D vectors from the cross-product of two 2D vectors? If code is to be provided, I can handle C#, JavaScript and some C++.
EDIT - here is a piece of the code in the book as I mentioned above:
int IntersectLineQuad(Point p, Point q, Point a, Point b, Point c, Point d, Point &r)
{
Vector pq = q - p;
Vector pa = a - p;
Vector pb = b - p;
Vector pc = c - p;
// Determine which triangle to test against by testing against diagonal first
Vector m = Cross(pc, pq);
float v = Dot(pa, m); // ScalarTriple(pq, pa, pc);
if (v >= 0.0f) {
// Test intersection against triangle abc
float u = -Dot(pb, m); // ScalarTriple(pq, pc, pb);
if (u < 0.0f) return 0;
float w = ScalarTriple(pq, pb, pa);
....
For the page you linked, it seems that they talk about a triangle in a 3d space:
Because the triangle can be oriented in any way in 3d-space, ...
Hence all the vectors they talk about are 3d vectors, and all the text and code makes perfect sense. Note that even for a 2d vectors everything also makes sense, if you consider a cross product to be a 3d vector pointing out of screen. And they mention it on the page too:
If you take the cross product of [B-A] and [p-A], you'll get a vector pointing out of the screen.
Their code is correct too, both for 2d and 3d cases:
function SameSide(p1,p2, a,b)
cp1 = CrossProduct(b-a, p1-a)
cp2 = CrossProduct(b-a, p2-a)
if DotProduct(cp1, cp2) >= 0 then return true
else return false
For 2d, both cp1 and cp2 are vectors pointing out of screen, and the (3d) dot product is exactly what you need to check; checking just the product of corresponding Z components is the same. If everything is 3d, this is also correct. (Though I would write simply return DotProduct(cp1, cp2) >= 0.)
For int IntersectLineQuad(), I can guess that the situation is the same: the Quad, whatever it is, is a 3d object, as well as Vector and Point in code. However, if you add more details about what is this function supposed to do, this will help.
In fact, it is obvious that any problem stated in 2d can be extended to 3d, and so any approach which is valid in 3d will also be valid for 2d case too, you just need to imagine a third axis pointing out of screen. So I think this is a valid (though confusing) technique to describe a 2d problem completely in 3d terms. You might yourself doing some extra work, because some values will always be zero in such an approach, but in turn the (almost) same code will work in a general 3d case too.

How to get the vector direction between two (p1, p2)?

I have been working on Unigine and been trying to code a flight program for weeks, I need to find the direction between two dummy nodes so I can use this direction to rotate the wings of the aircraft. Any explanation would be appreciated.
First you have to calculate the length of the distance between P1 and P2.
distance = abs(P2(y) - P1(y))
Then you can use the angular functions to calculate the angle.
a = sin(distance / length(P12))
As already stated in the comments, the vector from P1 to P2 is given by P = P2 - P1.
The direction can be attained in two ways.
1. Directly compute angle = tan_inverse( P.y() / P.x() ).
In this method however, 1st quadrant and 3rd quadrant are treated in the same way as the signs cancel out.
2.You can normalize this vector to get a unit vector. This is the preferred way since it alleviates the quadrant issues.
P(normalized) = P / (mod(p))
Now you can get the projection of any vector in this direction by just calculating the dot-product by this unit vector.
I just randomly came across this question and therefore it might be useful for someone else to read some more useful information here, regardless of the fact that this question has been asked years ago.
Currently, there is no accepted answer, which could mean that it wasn't very clear what the OP was asking so I'll confront two problems here.
Finding the direction of a vector
I am not very fluent in C++ so I'll
go one abstraction below and write C. Consider the following function
double get_vector_direction (vector v)
{
return atan2(v.q.y - v.p.y, v.q.x - v.p.x); /* atan(ratio) */
}
As simple as this! Also, I like to define vectors this way:
typedef struct POINT { double x, y; } point;
typedef struct VECTOR { point p, q; } vector;
the atan family of functions returns the inverse tangent and this function returns the direction of a vector, which is the measure of the angle it makes with a horizontal line (in radians).
There is a diagram in the answers here that shows the horizontal line as the x component of a vector. It's a simplistic implementation of a Cartesian to Polar coordinates conversion.
Finding the distance between the initial and terminal point
You may also want to know the magnitude of the vector, which could be obtained using the distance formula: sqrt(pow(v.q.x-v.p.x, 2)+pow(v.q.y-v.p.y, 2));
The two functions make up direction() and distance() which are the two most essential functions when dealing with algebra during game development. I would also recommend vectoradd and maybe even vectorsub and of course, radtodeg to convert radians to degrees if the angle is to be showed to the player.
If under the direction you mean angle w.r.t. an arbitrary vector (let it be (p1,p3)), then you can calculate the angle using:
angle = arcos( (p1,p2) * (p1,p3) / (modulus((p1,p2)) * modulus((p1,p3)) ))
where * is the dot product. The angle will be in radians. To change it to degrees you can multiply it by 180/PI (PI=3.1415926...). Modulus is length of vector:
modulus((p1,p2))=square root((p1,p2) * (p1,p2)).
The answer is rather about math than C++ but the implementation of the simple formula is straightforward.

C++ Collision using Obj Models

I'm experimenting with OpenGL 3.2+ and have starting loading Obj files/models in 3D and trying to interact with them.
(following tutorials from sources like this site)
I was wondering what the easiest way (if it's possible) to set up collision detection between two existing(loaded) Obj objects/Models without using third party physics engines etc?
The easiest possible algorithm that can meet your criteria detects collision between spheres, that concludes your meshes. Here you can see the implementation example.
Simplest collision model is to use bounding boxes for collision. The principle is simple: You surround your object by a box defined by two points, minimum and maximum. You then use these points to determine whether two boxes intersect.
In my engine the structure of bounding box and collision-detection method are set as this:
typedef struct BoundingBox
{
Vector3 min; //Contains lowest corner of the box
Vector3 max; //Contains highest corner of the box
} AABB;
//True if collision is detected, false otherwise
bool detectCollision( BoundingBox a, BoundingBox b )
{
return (a.min <= b.max && b.min <= a.max);
}
Other simple method is to use spheres. This method is useful for objects that are of similar size in all dimensions but it creates lots of false collisions if they are not. In this method, you surround your object by sphere with radius radius and center position position and when it comes to the collision, you simply check whether the distance between centers is smaller than sum of the radii and that case two spheres intersect.
Again, code snippet from my engine:
struct Sphere
{
Vector3 position; //Center of the sphere
float radius; //Radius of the sphere
};
bool inf::physics::detectCollision( Sphere a, Sphere b )
{
Vector3 tmp = a.position - b.position; //Distance between centers
return (Dot(tmp, tmp) <= pow((a.radius + b.radius), 2));
}
In code above Dot() computes the dot product of two vectors, if you dot vector with itself it gives you (by definition) the magnitude of the vector squared. Notice how I am actually not square-rooting to get the actual distances and I am comparing the squares instead to avoid extra computations.
You should also be aware that neither of these methods are perfect and will give you false collision detection (unless the objects are perfect boxes or spheres) from time to time but that is the trade-off of the simple implementation and computation complexity. Nevertheless it is good way to start detecting collisions.

Efficient C++ quaternion multiplication using cv::Mat

I want to multiply 2 quaternions, which are stored in a cv::Mat structure. I want the function to be as efficient as possible. I have the following code so far:
/** Quaternion multiplication
*
*/
void multiplyQuaternion(const Mat& q1,const Mat& q2, Mat& q)
{
// First quaternion q1 (x1 y1 z1 r1)
const float x1=q1.at<float>(0);
const float y1=q1.at<float>(1);
const float z1=q1.at<float>(2);
const float r1=q1.at<float>(3);
// Second quaternion q2 (x2 y2 z2 r2)
const float x2=q2.at<float>(0);
const float y2=q2.at<float>(1);
const float z2=q2.at<float>(2);
const float r2=q2.at<float>(3);
q.at<float>(0)=x1*r2 + r1*x2 + y1*z2 - z1*y2; // x component
q.at<float>(1)=r1*y2 - x1*z2 + y1*r2 + z1*x2; // y component
q.at<float>(2)=r1*z2 + x1*y2 - y1*x2 + z1*r2; // z component
q.at<float>(3)=r1*r2 - x1*x2 - y1*y2 - z1*z2; // r component
}
Is this the fastest way with OpenCV? Would it be fastest using fixed-point arithmetic?
In this tutorial different ways to access different pixels are covered. The Mat::at function was found to be about 10% slower in comparison to direct pixel access, probably due to the extra check in debug mode.
If you are really off for performance, you should rewrite your method with the 3 different methods mentioned in the text and then profile to find the one which is best in your situation.
There -had- been an ARM vector floating point quaternion multiply out there I can not find now. I could find this SIMD library:
Bullet 3D Game Multiphysics Library
Quaternions are often used to rotate 3D vectors so you might consider checking that one quaternion is a pure vector (i.e., the scalar or real part is zero). This could cut your work to 12 multiplies, 8 adds/subtracts and one sign flip.
You can also use quaternion multiplication on two pure vectors to compute their dot and cross products simultaneously, so testing for this special case may also be worth it. If both quaternions are pure vectors, you only need do 9 multiplies, 5 add/subtracts and one sign flip.