source: GTP/trunk/Lib/Vis/Preprocessing/src/Plane3.cpp @ 639

Revision 639, 1.6 KB checked in by mattausch, 19 years ago (diff)
Line 
1#include "Plane3.h"
2#include "Matrix4x4.h"
3
4
5
6
7bool
8PlaneIntersection(const Plane3 &a, const Plane3 &b, const Plane3 &c, Vector3 &result)
9{
10  Vector3
11    sx(a.mNormal.x, b.mNormal.x, c.mNormal.x),
12    sy(a.mNormal.y, b.mNormal.y, c.mNormal.y),
13    sz(a.mNormal.z, b.mNormal.z, c.mNormal.z),
14    sd(a.mD, b.mD, c.mD);
15
16  Matrix4x4 md(a.mNormal, b.mNormal, c.mNormal), mx, my, mz;
17
18  mx.SetColumns(sd, sy, sz);
19  my.SetColumns(sx, sd, sz);
20  mz.SetColumns(sx, sy, sd);
21 
22  double det = md.Det3x3();
23
24  if (abs(det)<TRASH)
25    return false;
26
27  result.SetValue(mx.Det3x3()/det,
28                  my.Det3x3()/det,
29                  mz.Det3x3()/det);
30 
31  return true;
32}
33
34
35Vector3 Plane3::FindIntersection(const Vector3 &a,                                                 
36                                                                 const Vector3 &b,
37                                                                 float *t,
38                                                                 bool *coplanar) const
39{
40    const Vector3 v = b - a; // line from A to B
41    float dv = DotProd(v, mNormal);
42   
43    if (signum(dv) == 0)
44        {
45                if (coplanar)
46                        (*coplanar) = true;     
47
48                if (t)
49                        (*t) = 0;
50
51                return a;
52        }
53       
54        if (coplanar)
55                (*coplanar) = false;
56
57    float u = - Distance(a) / dv; // TODO: could be done more efficiently
58   
59        if (t)
60                (*t) = u;
61       
62    return a + u * v;
63}
64
65
66int Plane3::Side(const Vector3 &v, const float threshold) const
67{
68    return signum(Distance(v), threshold);
69}
70
71
72float Plane3::FindT(const Vector3 &a, const Vector3 &b) const
73{         
74        const Vector3 v = b - a; // line from A to B
75        const float dv = DotProd(v, mNormal);
76   
77        // does not intersect or coincident
78
79        if (signum(dv) == 0)
80                return 0;
81       
82        return - Distance(a) / dv; // TODO: could be done more efficiently
83}
Note: See TracBrowser for help on using the repository browser.