[2751] | 1 | #include "Plane3.h"
|
---|
| 2 | #include "Matrix4x4.h"
|
---|
| 3 |
|
---|
| 4 |
|
---|
[2776] | 5 | namespace CHCDemoEngine
|
---|
[2751] | 6 | {
|
---|
| 7 |
|
---|
| 8 | Plane3::Plane3(const Vector3 &a, const Vector3 &b, const Vector3 &c)
|
---|
| 9 | {
|
---|
[2755] | 10 | Vector3 v1 = a - b, v2 = c - b;
|
---|
[2944] | 11 |
|
---|
| 12 | mNormal = Normalize(CrossProd(v2, v1));
|
---|
[2751] | 13 | mD = -DotProd(b, mNormal);
|
---|
| 14 | }
|
---|
| 15 |
|
---|
| 16 |
|
---|
| 17 | Plane3::Plane3(const Vector3 &normal, const Vector3 &point):
|
---|
| 18 | mNormal(normal)
|
---|
| 19 | {
|
---|
[2944] | 20 | //mNormal = Normalize(normal)
|
---|
[2751] | 21 | mD = -DotProd(normal, point);
|
---|
| 22 | }
|
---|
| 23 |
|
---|
| 24 |
|
---|
| 25 | bool PlaneIntersection(const Plane3 &a,
|
---|
| 26 | const Plane3 &b,
|
---|
| 27 | const Plane3 &c,
|
---|
| 28 | Vector3 &result)
|
---|
| 29 | {
|
---|
| 30 | Vector3 sx(a.mNormal.x, b.mNormal.x, c.mNormal.x),
|
---|
| 31 | sy(a.mNormal.y, b.mNormal.y, c.mNormal.y),
|
---|
[2755] | 32 | sz(a.mNormal.z, b.mNormal.z, c.mNormal.z),
|
---|
[2944] | 33 | sd(a.mD, b.mD, c.mD);
|
---|
[2751] | 34 |
|
---|
[2755] | 35 | Matrix4x4 md(a.mNormal, b.mNormal, c.mNormal), mx, my, mz;
|
---|
[2751] | 36 |
|
---|
[2755] | 37 | mx.SetColumns(sd, sy, sz);
|
---|
| 38 | my.SetColumns(sx, sd, sz);
|
---|
| 39 | mz.SetColumns(sx, sy, sd);
|
---|
[2751] | 40 |
|
---|
[2755] | 41 | const float det = md.Det3x3();
|
---|
[2751] | 42 |
|
---|
[2755] | 43 | if (fabs(det) < TRASH)
|
---|
| 44 | return false;
|
---|
| 45 |
|
---|
[2944] | 46 | result.SetValue(mx.Det3x3() / det, my.Det3x3() / det, mz.Det3x3() / det);
|
---|
[2755] | 47 |
|
---|
| 48 | return true;
|
---|
[2751] | 49 | }
|
---|
| 50 |
|
---|
| 51 |
|
---|
[2755] | 52 | bool PlaneIntersection(const Plane3 &p1, const Plane3 &p2)
|
---|
| 53 | {
|
---|
| 54 | return
|
---|
| 55 | p1.mNormal.x != p2.mNormal.x ||
|
---|
| 56 | p1.mNormal.y != p2.mNormal.y ||
|
---|
| 57 | p1.mNormal.z != p2.mNormal.z ||
|
---|
| 58 | p1.mD == p2.mD;
|
---|
| 59 | }
|
---|
[2751] | 60 |
|
---|
| 61 |
|
---|
| 62 | Vector3 Plane3::FindIntersection(const Vector3 &a,
|
---|
| 63 | const Vector3 &b,
|
---|
| 64 | float *t,
|
---|
| 65 | bool *coplanar) const
|
---|
| 66 | {
|
---|
| 67 | // line from A to B
|
---|
| 68 | const Vector3 v = b - a;
|
---|
| 69 | float dv = DotProd(v, mNormal);
|
---|
| 70 |
|
---|
| 71 | if (signum(dv) == 0)
|
---|
| 72 | {
|
---|
| 73 | if (coplanar) (*coplanar) = true;
|
---|
| 74 |
|
---|
| 75 | if (t) (*t) = 0;
|
---|
| 76 |
|
---|
| 77 | return a;
|
---|
| 78 | }
|
---|
| 79 |
|
---|
| 80 | if (coplanar) (*coplanar) = false;
|
---|
| 81 |
|
---|
| 82 | float u = - Distance(a) / dv;
|
---|
| 83 |
|
---|
| 84 | if (t) (*t) = u;
|
---|
| 85 |
|
---|
| 86 | return a + u * v;
|
---|
| 87 | }
|
---|
| 88 |
|
---|
| 89 |
|
---|
| 90 | int Plane3::Side(const Vector3 &v, const float threshold) const
|
---|
| 91 | {
|
---|
| 92 | return signum(Distance(v), threshold);
|
---|
| 93 | }
|
---|
| 94 |
|
---|
| 95 |
|
---|
| 96 | float Plane3::FindT(const Vector3 &a, const Vector3 &b) const
|
---|
| 97 | {
|
---|
| 98 | const Vector3 v = b - a; // line from A to B
|
---|
| 99 | const float dv = DotProd(v, mNormal);
|
---|
| 100 |
|
---|
| 101 | // does not intersect or coincident
|
---|
| 102 | if (signum(dv) == 0)
|
---|
| 103 | return 0;
|
---|
| 104 |
|
---|
| 105 | return - Distance(a) / dv;
|
---|
| 106 | }
|
---|
| 107 |
|
---|
| 108 |
|
---|
[2944] | 109 | void Plane3::Transform(const Matrix4x4 &m)
|
---|
| 110 | {
|
---|
| 111 | float w;
|
---|
| 112 |
|
---|
| 113 | const Matrix4x4 invTranspose = Transpose(Invert(m));
|
---|
| 114 |
|
---|
| 115 | const Vector3 n = invTranspose.Transform(w, mNormal, -mD);
|
---|
| 116 |
|
---|
| 117 | const float mag = Magnitude(n);
|
---|
| 118 |
|
---|
| 119 | mNormal = n / mag;
|
---|
| 120 | // our d is -d of homogen equation
|
---|
| 121 | mD = -w / mag;
|
---|
| 122 |
|
---|
[2945] | 123 | /*
|
---|
[2944] | 124 | // find point on plane
|
---|
[2945] | 125 | const Vector3 pt = mNormal * -mD;
|
---|
[2944] | 126 |
|
---|
| 127 | // transform point
|
---|
| 128 | const Vector3 newPt = m * pt;
|
---|
[2945] | 129 |
|
---|
[2944] | 130 | // transform normal
|
---|
[2945] | 131 | nNormal = Normalize(TransformNormal(m, newPt));
|
---|
[2944] | 132 |
|
---|
[2945] | 133 | mD = -DotProd(mNormal, newPt);
|
---|
| 134 | */
|
---|
[2751] | 135 | }
|
---|
[2944] | 136 |
|
---|
| 137 |
|
---|
| 138 | }
|
---|