SHARE
TWEET

planeCoefficients

Archon Feb 20th, 2012 67 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. //Three doubles (x, y, z)
  2. struct D3{
  3.    double x;
  4.    double y;
  5.    double z;
  6.  
  7.    D3(double x_ = 0, double y_ = 0, double z_ = 0):
  8.    x(x_), y(y_), z(z_){}
  9.  
  10.    D3 operator-(const D3 &rhs) const{
  11.       return D3(x - rhs.x,
  12.                 y - rhs.y,
  13.                 z - rhs.z);
  14.    }
  15.  
  16.    D3 operator-() const{
  17.       return D3(-x, -y, -z);
  18.    }
  19.  
  20.    D3 operator/=(double s) const{
  21.       return D3(x/s, y/s, z/s);
  22.    }
  23. };
  24.  
  25. //Four doubles (a, b, c, d)
  26. struct D4{
  27.    double a;
  28.    double b;
  29.    double c;
  30.    double d;
  31.  
  32.    D4(double a_ = 0, double b_ = 0, double c_ = 0, double d_ = 0):
  33.    a(a_), b(b_), c(c_), d(d_){}
  34. };
  35.  
  36. double determinantFromVectors(const D3 &x, const D3 &y, const D3 &z){
  37.    return
  38.       + x.x * y.y * z.z
  39.       + y.x * z.y * x.z
  40.       + z.x * x.y * y.z
  41.  
  42.       - z.x * y.y * x.z
  43.       - y.x * x.y * z.z
  44.       - x.x * z.y * y.z;
  45. }
  46.  
  47. D3 crossProduct(const D3 &u, const D3 &v){
  48.    return D3(u.y * v.z - u.z * v.y,
  49.              u.z * v.x - u.x * v.z,
  50.              u.x * v.y - u.y * v.z);
  51. }
  52.  
  53. void normalize(D3 &v){
  54.    double length = sqrt(v.x * v.x +
  55.                         v.y * v.y +
  56.                         v.z * v.z);
  57.    v /= length;
  58. }
  59.  
  60. D4 planeCoefficients(const D3 &p1, const D3 &p2, const D3 &p3){
  61.    //Option 1: Matrix determinants
  62.    {
  63.       // v1 vX vY vZ
  64.       // -----------
  65.       //  1 x1 y1 z1
  66.       //  1 x2 y2 z2
  67.       //  1 x3 y3 z3
  68.       D3
  69.          v1(1, 1, 1),
  70.          vX(p1.x, p2.x, p3.x),
  71.          vY(p1.y, p2.y, p3.y),
  72.          vZ(p1.z, p2.z, p3.z);
  73.  
  74.       return D4(determinantFromVectors(v1, vY, vZ),
  75.                 determinantFromVectors(vX, v1, vZ),
  76.                 determinantFromVectors(vX, v1, vZ),
  77.                -determinantFromVectors(vX, vY, vZ));
  78.    }
  79.  
  80.    //Option 2: Cross-product of vectors
  81.    {
  82.       //   |
  83.       //   |
  84.       //   |
  85.       // p1|__________p2
  86.       //    \    v2
  87.       //     \v1
  88.       //      \
  89.       //       \p3
  90.       D3
  91.          v1 = p3 - p1,
  92.          v2 = p2 - p1,
  93.          cross = crossProduct(v1, v2);
  94.       normalize(cross);
  95.  
  96.       return D4(cross.x,
  97.                 cross.y,
  98.                 cross.z,
  99.  
  100.                 cross.x * p3.x +
  101.                 cross.y * p3.y +
  102.                 cross.z * p3.z);
  103.    }
  104. }
RAW Paste Data
Top