MCDV/plane.h

   1 #pragma once
   2 #include <iostream>
   3 #include <glad\glad.h>
   4 #include <GLFW\glfw3.h>
   5
   6 #include <glm\glm.hpp>
   7 #include <glm\gtc\matrix_transform.hpp>
   8 #include <glm\gtc\type_ptr.hpp>
   9
  10 #include <vector>
  11
  12 #include <math.h>
  13
  14 class Plane {
  15 public:
  16         glm::vec3 normal;
  17         float offset;
  18
  19         int textureID = 0;
  20
  21         //Plane defined by three points
  22         Plane(glm::vec3 P, glm::vec3 Q, glm::vec3 R) {
  23                 glm::vec3 n = glm::cross(P - Q, P - R); //Calculate normal
  24                                                                                                 //float d = ((P.x * n.x + P.y * n.y + P.z * n.z) / glm::length(n)); //Calculate distance
  25
  26                 float d = glm::dot(glm::normalize(n), P);
  27
  28                 //Set class attribs
  29                 this->normal = glm::normalize(n);
  30                 this->offset = d;
  31         }
  32
  33         //Direct constructor
  34         Plane(glm::vec3 normal, float offset) {
  35                 this->offset = offset;
  36                 this->normal = normal;
  37         }
  38
  39         //Standard constructor (generic floor plane)
  40         Plane() {
  41                 this->offset = 0.0f;
  42                 this->normal = glm::vec3(0, 0, 1);
  43         }
  44
  45         //Destructor
  46         ~Plane() {};
  47
  48         static bool ThreePlaneIntersection(Plane p1, Plane p2, Plane p3, glm::vec3* p) {
  49                 float det = glm::dot(glm::cross(p1.normal, p2.normal), p3.normal);
  50
  51                 float epsilon = 1e-5f; //Epsilon value for floating point error
  52
  53                 if (det < epsilon && det > -epsilon) { return false; };
  54
  55                 *p = (-(p1.offset * glm::cross(p2.normal, p3.normal)) -
  56                         (p2.offset * glm::cross(p3.normal, p1.normal)) -
  57                         (p3.offset * glm::cross(p1.normal, p2.normal))) / det;
  58                 return true;
  59         }
  60
  61         static bool FinalThreePlaneIntersection(Plane p1, Plane p2, Plane p3, glm::vec3* p) {
  62                 float det = glm::dot(glm::cross(p1.normal, p2.normal), p3.normal);
  63                 float epsilon = 1e-5f; //Epsilon value for floating point error
  64
  65                 if (det < epsilon && det > -epsilon) { return false; };
  66
  67                 *p = -(-p1.offset * glm::cross(p2.normal, p3.normal)
  68                         - p2.offset * glm::cross(p3.normal, p1.normal)
  69                         - p3.offset * glm::cross(p1.normal, p2.normal))
  70                         / glm::dot(p1.normal, glm::cross(p2.normal, p3.normal));
  71
  72                 return true;
  73
  74                 /*
  75
  76                 p = (-adist(Cross(bnorm, cnorm))
  77                 -bdist(Cross(cnorm, anorm))
  78                 -cdist(Cross(anorm, bnorm)))
  79                 / Dot(anorm, Cross(bnorm, cnorm))
  80
  81                 */
  82
  83         }
  84
  85         static bool GetTripleIntersection(Plane p1, Plane p2, Plane p3, glm::vec3* p) {
  86                 float determinant = glm::determinant(glm::mat3{ p1.normal.x, p2.normal.x, p3.normal.x,
  87                         p1.normal.y, p2.normal.y, p3.normal.y,
  88                         p1.normal.z, p2.normal.z, p3.normal.z });
  89
  90                 float epsilon = 1e-5f; //Check within small epsilon to prevent infinite values
  91                 if (determinant < epsilon && determinant > -epsilon) return false;
  92
  93                 glm::vec3 point = (glm::cross(p2.normal, p3.normal) * -p1.offset +
  94                         glm::cross(p3.normal, p1.normal) * -p2.offset +
  95                         glm::cross(p1.normal, p2.normal) * -p3.offset) / determinant;
  96
  97                 *p = point;
  98                 return true;
  99
 100                 /*
 101                 Determinant layout
 102                 -- plane normals --
 103                 \ N1  N2  N3
 104                 x
 105                 y
 106                 z
 107                 ^components */
 108         }
 109
 110         //Detects which side of the plane the point is on (-): negative, (~0): coplanar, (+): positive
 111         static float EvalPointPolarity(Plane plane, glm::vec3 p1) {
 112                 return glm::dot(p1, plane.normal) - //Evaluate dot & plane normal
 113                         glm::dot(plane.normal * plane.offset, plane.normal); //Compare to known point & plane normal (closest point to 0,0,0 is normal*offset)
 114         }
 115
 116         //Determines clockwise-ness of point B in relation to point A with respect to center point C
 117         static float CompareClockWiseNess(Plane plane, glm::vec3 C, glm::vec3 A, glm::vec3 B) {
 118                 return glm::dot(plane.normal, glm::cross(A - C, B - C));
 119         }
 120
 121         static std::vector<glm::vec3> OrderCoplanarClockWise(Plane plane, std::vector<glm::vec3> Points) {
 122                 //Find center point (avarage distribution of points)
 123                 glm::vec3 center(0, 0, 0);
 124                 for (int i = 0; i < Points.size(); i++) {
 125                         center += Points[i];
 126                 }
 127                 center /= Points.size();
 128
 129                 glm::vec3 ref = Points[0] - center;
 130
 131                 std::vector<glm::vec4> angledVecs;
 132
 133
 134                 for (int i = 0; i < Points.size(); i++) {
 135                         glm::vec3 diff = Points[i] - center;
 136                         float ang = atan2(glm::length(glm::cross(diff, ref)), glm::dot(diff, ref));
 137
 138                         float sign = glm::dot(glm::cross(diff, ref), plane.normal) < 0 ? -1.0f : 1.0f;
 139                         ang *= sign;
 140
 141                         angledVecs.push_back(glm::vec4(Points[i].x, Points[i].y, Points[i].z, ang));
 142                 }
 143
 144                 while (true)
 145                 {
 146                         bool modified = false;
 147
 148                         for (int i = 0; i < Points.size() - 1; i++)
 149                         {
 150                                 int s0 = i; int s1 = i + 1;
 151
 152                                 glm::vec4 a = angledVecs[s0]; glm::vec4 b = angledVecs[s1];
 153
 154                                 if (a.w > b.w)
 155                                 {
 156                                         angledVecs[s0] = b; angledVecs[s1] = a;
 157                                         modified = true;
 158                                 }
 159                         }
 160                         if (!modified) break;
 161                 }
 162
 163                 for (int i = 0; i < Points.size(); i++)
 164                 {
 165                         Points[i] = glm::vec3(angledVecs[i].x, angledVecs[i].y, angledVecs[i].z);
 166                 }
 167
 168                 return Points;
 169         }
 170
 171         static void InPlaceOrderCoplanarClockWise(Plane plane, std::vector<glm::vec3>* Points){
 172                 if (Points->size() == 0) return;
 173
 174                 //Find center point (avarage distribution of points)
 175                 glm::vec3 center(0, 0, 0);
 176                 for (int i = 0; i < Points->size(); i++) {
 177                         center += (*Points)[i];
 178                 }
 179                 center /= Points->size();
 180
 181                 glm::vec3 ref = (*Points)[0] - center;
 182
 183                 std::vector<glm::vec4> angledVecs;
 184
 185                 for (int i = 0; i < Points->size(); i++) {
 186                         glm::vec3 diff = (*Points)[i] - center;
 187                         float ang = atan2(glm::length(glm::cross(diff, ref)), glm::dot(diff, ref));
 188
 189                         float sign = glm::dot(glm::cross(diff, ref), plane.normal) < 0 ? -1.0f : 1.0f;
 190                         ang *= sign;
 191
 192                         angledVecs.push_back(glm::vec4((*Points)[i].x, (*Points)[i].y, (*Points)[i].z, ang));
 193                 }
 194
 195                 while (true){
 196                         bool modified = false;
 197
 198                         for (int i = 0; i < Points->size() - 1; i++){
 199                                 int s0 = i; int s1 = i + 1;
 200
 201                                 glm::vec4 a = angledVecs[s0]; glm::vec4 b = angledVecs[s1];
 202
 203                                 if (a.w > b.w){
 204                                         angledVecs[s0] = b; angledVecs[s1] = a;
 205                                         modified = true;
 206                                 }
 207                         }
 208                         if (!modified) break;
 209                 }
 210
 211                 for (int i = 0; i < Points->size(); i++){
 212                         (*Points)[i] = glm::vec3(angledVecs[i].x, angledVecs[i].y, angledVecs[i].z);
 213                 }
 214         }
 215 };