https://github.com/merveasiler/Geometric-Kernel-Approximation
Tip revision: ad8e9d4f15917a78050e26fb7a846e17f90636e6 authored by Merve Asiler on 03 July 2024, 20:45:15 UTC
Update README.md
Update README.md
Tip revision: ad8e9d4
BaseGeoOpUtils.cpp
// @author Merve Asiler
#include "BaseGeoOpUtils.h"
#include <iostream>
#include <Eigen/Dense>
int findPeakVertexID(Triangle* triangle, Edge* edge) {
for (unsigned int i = 0; i < 3; i++) {
if (triangle->corners[i] == edge->endVerts[0] || triangle->corners[i] == edge->endVerts[1])
continue;
return triangle->corners[i];
}
return INVALID_TRIANGLE; // "not found" is not a possible case
}
double* findLine2LineIntersection(Line* line1, Line* line2) { // Assume that they have a certain intersection point
Eigen::Matrix2d A;
A << line2->directionVector[0], -line1->directionVector[0],
line2->directionVector[1], -line1->directionVector[1];
Eigen::Vector2d b;
b << line1->point[0] - line2->point[0],
line1->point[1] - line2->point[1];
Eigen::Vector2d x = A.colPivHouseholderQr().solve(b);
double* point = new double[3];
for (int i = 0; i < 3; i++)
point[i] = line1->point[i] + line1->directionVector[i] * x[1];
return point;
}
double findLinePlaneIntersection(Line& line, Plane& plane) {
/* Assume they intersect at q = p + v*t (the rhs is parametric line equation)
A * q.x + B * q.y + C * q.z + D = 0 (comes from the plane equation)
*/
double midProduct = dotProduct(plane.ABCD, line.point);
double numerator = midProduct + plane.ABCD[3];
double denominator = dotProduct(plane.ABCD, line.directionVector);
if (abs(denominator) < 3 * EPSILON)
return numeric_limits<double>::infinity();
return -numerator / denominator;
}
double findRayTriangleIntersection(Line& line, Triangle& triangle, Plane& trianglePlane) { // assumes the triangle's normal is precomputed
double coeff = findLinePlaneIntersection(line, trianglePlane);
if (isinf(coeff))
return numeric_limits<double>::infinity();
double Q[3]; // intersection point
for (int k = 0; k < 3; k++)
Q[k] = line.point[k] + coeff * line.directionVector[k];
// compute barycentric coordinates of Q
double* baryCoords = triangle.computeBarycentricCoords(Q, trianglePlane.point1, trianglePlane.point2, trianglePlane.point3);
if (baryCoords == NULL)
return numeric_limits<double>::infinity();
delete[] baryCoords;
return coeff;
}
double findRayTriangleIntersection(Line* ray, TriangleWithVerts* triangleWV) {
triangleWV->computeNormal(triangleWV->vertices[0], triangleWV->vertices[1], triangleWV->vertices[2]);
Plane* plane = new Plane(triangleWV->vertices[0]->coords, triangleWV->normal);
double coeff = findLinePlaneIntersection(*ray, *plane);
delete plane;
if (isinf(coeff)) {
return numeric_limits<double>::infinity();
}
double Q[3]; // intersection point
for (int k = 0; k < 3; k++)
Q[k] = ray->point[k] + coeff * ray->directionVector[k];
// compute barycentric coordinates of Q
double* baryCoords = triangleWV->computeBarycentricCoords(Q);
if (baryCoords == NULL)
return numeric_limits<double>::infinity();
delete[] baryCoords;
return coeff;
}
Line* find2PlaneIntersection(Plane& plane1, Plane& plane2) {
double cosAngle = dotProduct(plane1.ABCD, plane2.ABCD);
if (fabs(fabs(cosAngle) - 1) < EPSILON)
return NULL;
double dirVector[3];
crossProduct(plane1.ABCD, plane2.ABCD, dirVector);
normalize(dirVector);
bool point_found = false;
double point[3];
double coeffs[2][4] = { {plane1.ABCD[0], plane1.ABCD[1], plane1.ABCD[2], plane1.ABCD[3]}, {plane2.ABCD[0], plane2.ABCD[1], plane2.ABCD[2], plane2.ABCD[3]} };
for (int i = 0; i < 3; i++) {
double det = coeffs[0][i] * coeffs[1][(i + 1) % 3] - coeffs[0][(i + 1) % 3] * coeffs[1][i];
if (abs(det) < 2 * EPSILON)
continue;
point[i] = (-coeffs[1][(i + 1) % 3] * coeffs[0][3] + coeffs[0][(i + 1) % 3] * coeffs[1][3]) / det;
if (abs(coeffs[0][(i + 1) % 3]) < EPSILON)
point[(i + 1) % 3] = (-coeffs[1][3] - point[i] * coeffs[1][i]) / coeffs[1][(i + 1) % 3];
else
point[(i + 1) % 3] = (-coeffs[0][3] - point[i] * coeffs[0][i]) / coeffs[0][(i + 1) % 3];
point[(i + 2) % 3] = 0;
point_found = true;
break;
}
if (point_found)
return new Line(point, dirVector);
return NULL;
}
double* find3PlaneIntersection(Plane& plane1, Plane& plane2, Plane& plane3) {
Line* line = find2PlaneIntersection(plane1, plane2);
if (line == NULL)
return NULL;
double t = findLinePlaneIntersection(*line, plane3);
if (t == numeric_limits<double>::infinity()) {
delete line;
return NULL;
}
double* point = new double[3];
for (int i = 0; i < 3; i++)
point[i] = line->directionVector[i] * t + line->point[i];
delete line;
return point;
/*
// Cramer's rule
double ABC[3][3] = { {planes[0].ABCD[0], planes[0].ABCD[1], planes[0].ABCD[2]},
{planes[1].ABCD[0], planes[1].ABCD[1], planes[1].ABCD[2]},
{planes[2].ABCD[0], planes[2].ABCD[1], planes[2].ABCD[2]} };
double detABC = computeDeterminant3x3(ABC);
if (abs(detABC) < 3*EPSILON*EPSILON)
return NULL;
double ABC_x[3][3], ABC_y[3][3], ABC_z[3][3];
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
ABC_x[i][j] = ABC[i][j];
ABC_y[i][j] = ABC[i][j];
ABC_z[i][j] = ABC[i][j];
}
ABC_x[i][0] = -planes[i].ABCD[3];
ABC_y[i][1] = -planes[i].ABCD[3];
ABC_z[i][2] = -planes[i].ABCD[3];
}
double* xyz = new double[3];
double dets[3] = { computeDeterminant3x3(ABC_x), computeDeterminant3x3(ABC_y), computeDeterminant3x3(ABC_z) };
for (int i = 0; i < 3; i++)
xyz[i] = dets[i] / detABC;
return xyz;
*/
}
double* projectVertexOnPlane(Plane* plane, double* vertex) {
double* point = new double[3];
double A = plane->ABCD[0];
double B = plane->ABCD[1];
double C = plane->ABCD[2];
double D = plane->ABCD[3];
double Px = vertex[0];
double Py = vertex[1];
double Pz = vertex[2];
/* If the projection coordinates are Q=(x,y,z),
then P-Q = t*N, where N is the plane normal.
In other words:
(x - Px, y - Py, z - Pz) = t * (Nx, Ny, Nz)
This results in 3 equations:
x = Px + t * Nx
y = Py + t * Ny
z = Pz + t * Nz
Put Q=(xy,z) into the plane equation:
A * (Px + t * Nx) + B * (Py + t * Ny) + C * (Pz + t * Nz) + D = 0
=> t = -(A*Px + B*Py + C*Pz + D) / (A*Nx + B*Ny + C*Nz)
Do not forget that <Nx, Ny, Nz> = <A, B, C>.
*/
double t = -(A * Px + B * Py + C * Pz + D) / (A * A + B * B + C * C);
for (int i = 0; i < 3; i++)
point[i] = vertex[i] + t * plane->ABCD[i];
/*
// TO-DO: What if either one of A,B,C is 0?
double tempProductY = (A * Py - B * Px);
double tempProductZ = (A * Pz - C * Px);
point[0] = -(D * A + (B * tempProductY + C * tempProductZ)) / (A * A + B * B + C * C);
point[1] = (tempProductY + B * point[0]) / A;
point[2] = (tempProductZ + C * point[0]) / A;
*/
return point;
}
/*
Is the given 3D point inside the region enclosed by the given half space?
returns 1 if the point is inside the region
returns 0 if the point is on the boundary of the region
returns -1 if the point is outside the region
*/
int isPointInRegion(const double* point, const HalfSpace* halfSpace) {
double total = halfSpace->ABCD[3];
for (unsigned int j = 0; j < 3; j++)
total += halfSpace->ABCD[j] * point[j];
if (abs(total) < EPSILON)
return 0; // on the boundary
else if (halfSpace->isLargerThanZero && total >= 0)
return 1; // inside
else if (!halfSpace->isLargerThanZero && total <= 0)
return 1; // inside
else
return -1; // outside
}
double findClosestValueSatisfiedByPoint(const double point[3], const vector<HalfSpace>& halfSpaceContainer) {
double closestValue = numeric_limits<double>::infinity();
double furthestValue = -numeric_limits<double>::infinity();
for (unsigned int i = 0; i < halfSpaceContainer.size(); i++) {
HalfSpace halfSpace = halfSpaceContainer[i];
double total = halfSpace.ABCD[3];
for (unsigned int j = 0; j < 3; j++) {
double mult = halfSpace.ABCD[j] * point[j];
total += mult;
}
if ( (halfSpace.isLargerThanZero && total >= 0) || (!halfSpace.isLargerThanZero && total <= 0) ) {
if (abs(total) - closestValue < 3*EPSILON)
closestValue = abs(total);
}
else {
if (furthestValue - abs(total) < 3*EPSILON)
furthestValue = abs(total);
}
}
if (furthestValue > 0)
return furthestValue;
return -closestValue;
}
double* findValueVectorSatisfiedByPoint(const double point[3], const vector<HalfSpace>& halfSpaceContainer) {
double* valuesVector = new double[halfSpaceContainer.size()];
for (unsigned int i = 0; i < halfSpaceContainer.size(); i++) {
double total = halfSpaceContainer[i].ABCD[3];
for (unsigned int j = 0; j < 3; j++) {
double mult = halfSpaceContainer[i].ABCD[j] * point[j];
total += mult;
}
valuesVector[i] = total;
}
return valuesVector;
}
double isPointInHalfSpace(HalfSpace& halfspace, double* point) {
double distance = halfspace.ABCD[3];
for (unsigned int j = 0; j < 3; j++) {
double mult = halfspace.ABCD[j] * point[j];
distance += mult;
}
return distance;
}
void computeHalfSpacesFromTriangles(const vector<Triangle>& tris, const vector<Vertex>& verts, vector<HalfSpace>& halfSpaces) {
for (int i = 0; i < tris.size(); i++) {
Triangle tri = tris[i];
HalfSpace halfSpace(verts[tri.corners[0]].coords, tri.normal, false);
halfSpaces.push_back(halfSpace);
}
}
vector<double*> computeHalfSpaceCoeffsFromTriangles(const vector<Triangle>& tris, const vector<Vertex>& verts) {
vector<double*> halfSpaceCoeffs;
vector<HalfSpace> halfSpaces;
computeHalfSpacesFromTriangles(tris, verts, halfSpaces);
for (int i = 0; i < halfSpaces.size(); i++) {
double* coeffs = new double[4];
for (int j = 0; j < 4; j++)
coeffs[j] = halfSpaces[i].ABCD[j];
halfSpaceCoeffs.push_back(coeffs);
}
halfSpaces.clear();
return halfSpaceCoeffs;
}
double findAngleBetweenPlaneAndVector(const Plane* plane, const double* direction) {
double cosAngleWithNormal = findCosAngleBetween(plane->ABCD, direction);
double angleWithNormal = acos(cosAngleWithNormal);
return abs(angleWithNormal - M_PI_2);
}
double* projectVectorOnPlane(const Plane* plane, const double* direction) {
double q[3]; // represents the point obtained by going along the <direction> from <plane->point>
for (int i = 0; i < 3; i++)
q[i] = plane->point[i] + direction[i];
double angle = findAngleBetweenPlaneAndVector(plane, direction);
double height = computeLength(direction) * abs(sin(angle)); // height of q to the plane
double p[3]; // projection of q on to the plane
for (int i = 0; i < 3; i++)
p[i] = q[i] - plane->ABCD[i] * height;
double* projection = new double[3]; // projection of <direction> on to the <plane>
for (int i = 0; i < 3; i++)
projection[i] = p[i] - plane->point[i];
return projection;
}
HalfPlane* projectHalfSpaceOnPlane(const Plane* basePlane, const HalfSpace* hs) {
double* directionOfLine = crossProduct(basePlane->ABCD, hs->ABCD); // direction of the line occuring with the intersection of two planes
// Check whether the two planes are parallel (or the same) or not
bool is_parallel = true;
for (int i = 0; i < 3; i++) // if directionOfLine is <0, 0, 0>
if (abs(directionOfLine[i]) > EPSILON)
is_parallel = false;
if (is_parallel)
throw PARALLEL_PLANE_EXCEPTION();
// Find a common point of two planes:
Eigen::MatrixXd A(2,3);
A << basePlane->ABCD[0], basePlane->ABCD[1], basePlane->ABCD[2],
hs->ABCD[0], hs->ABCD[1], hs->ABCD[2];
Eigen::Vector2d b;
b << -basePlane->ABCD[3], -hs->ABCD[3];
Eigen::Vector3d x = A.colPivHouseholderQr().solve(b);
double point[3] = {x[0], x[1], x[2]};
// lastly, compute the normal of half plane (on the basePlane) and construct it
double* normalOnPlane = projectVectorOnPlane(basePlane, hs->ABCD);
normalize(normalOnPlane);
HalfPlane* projection = new HalfPlane(point, directionOfLine, normalOnPlane, false);
// clean-up
delete[] directionOfLine;
directionOfLine = nullptr;
delete[] normalOnPlane;
normalOnPlane = nullptr;
return projection;
}
double* rotate3x1(double rotationMatrix[3][3], double element3x1[3]) {
double* rotated3x1 = new double[3];
for (int i = 0; i < 3; i++) {
rotated3x1[i] = 0;
for (int j = 0; j < 3; j++)
rotated3x1[i] += rotationMatrix[i][j] * element3x1[j];
}
return rotated3x1;
}
bool isZeroVector(double* vect) {
bool result = true;
for (int i = 0; i < 3; i++)
result = result && (abs(vect[i]) < EPSILON ? true : false);
return result;
}
bool isTripleSame(double* p1, double* p2) {
if (abs(p1[0] - p2[0]) < EPSILON && abs(p1[1] - p2[1]) < EPSILON && abs(p1[2] - p2[2]) < EPSILON)
return true;
return false;
}
double** computeBoxCoords(double minCoords[3], double maxCoords[3]) {
double** coords = new double* [8];
// ---> handle the corners in the ordering taken by first change in x, then in z, then in y
coords[0] = new double[3]{ minCoords[0], minCoords[1], minCoords[2] };
coords[1] = new double[3]{ maxCoords[0], minCoords[1], minCoords[2] };
coords[2] = new double[3]{ maxCoords[0], minCoords[1], maxCoords[2] };
coords[3] = new double[3]{ minCoords[0], minCoords[1], maxCoords[2] };
coords[4] = new double[3]{ minCoords[0], maxCoords[1], minCoords[2] };
coords[5] = new double[3]{ maxCoords[0], maxCoords[1], minCoords[2] };
coords[6] = new double[3]{ maxCoords[0], maxCoords[1], maxCoords[2] };
coords[7] = new double[3]{ minCoords[0], maxCoords[1], maxCoords[2] };
return coords;
}