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

Revision 1990, 4.0 KB checked in by mattausch, 18 years ago (diff)
Line 
1#include "Plane3.h"
2#include "Matrix4x4.h"
3#include "Ray.h"
4
5
6namespace GtpVisibilityPreprocessor {
7
8
9Plane3::Plane3(const Vector3 &a,
10                           const Vector3 &b,
11                           const Vector3 &c)
12{
13    Vector3 v1=a-b, v2=c-b;
14    mNormal = Normalize(CrossProd(v2,v1));
15    mD = -DotProd(b, mNormal);
16}
17
18Plane3::Plane3(const Vector3 &normal,
19                           const Vector3 &point):
20mNormal(normal)
21{
22    mD = -DotProd(normal, point);
23}
24
25
26bool
27PlaneIntersection(const Plane3 &a, const Plane3 &b, const Plane3 &c, Vector3 &result)
28{
29  Vector3
30    sx(a.mNormal.x, b.mNormal.x, c.mNormal.x),
31    sy(a.mNormal.y, b.mNormal.y, c.mNormal.y),
32    sz(a.mNormal.z, b.mNormal.z, c.mNormal.z),
33    sd(a.mD, b.mD, c.mD);
34
35  Matrix4x4 md(a.mNormal, b.mNormal, c.mNormal), mx, my, mz;
36
37  mx.SetColumns(sd, sy, sz);
38  my.SetColumns(sx, sd, sz);
39  mz.SetColumns(sx, sy, sd);
40 
41  double det = md.Det3x3();
42
43  if (abs(det)<TRASH)
44    return false;
45
46  result.SetValue(mx.Det3x3()/det,
47                  my.Det3x3()/det,
48                  mz.Det3x3()/det);
49 
50  return true;
51}
52
53
54 bool PlaneIntersection(const Plane3 &p1, const Plane3 &p2)
55 {
56         return
57                 p1.mNormal.x != p2.mNormal.x ||
58                 p1.mNormal.y != p2.mNormal.y ||
59                 p1.mNormal.z != p2.mNormal.z ||
60                 p1.mD == p2.mD;
61 }
62
63
64/*
65If the planes are known to intersect then determine the origin and unit direction vector of a
66line formed by the intersection of two planes.
67The direction of that line is just the vector product of the normals of the planes.
68Finding a point along the intersection line is theoretically easy but numerical precision
69issues mean that we need to determine which axis it is best to do the calculation in.
70The point will lie on the axis that was used to determine the intersection.
71*/
72SimpleRay GetPlaneIntersection(const Plane3 &plane1, const Plane3 &plane2)
73{
74        Vector3 point, dir;
75        dir = CrossProd(plane1.mNormal, plane2.mNormal);
76   
77        const int index = dir.DrivingAxis();
78   
79        switch (index)
80        {
81        case 0:
82                point[0] = 0.0f;
83                point[1] = (plane1.mNormal[2] * plane2.mD - plane2.mNormal[2] * plane1.mD) / dir[0] ;
84                point[2] = (plane2.mNormal[1] * plane1.mD - plane1.mNormal[1] * plane2.mD) / dir[0] ;
85                break;
86        case 1:
87                point[0] = (plane2.mNormal[2] * plane1.mD - plane1.mNormal[2] * plane2.mD) / dir[1] ;
88                point[1] = 0.0f ;
89                point[2] = (plane1.mNormal[0] * plane2.mD - plane2.mNormal[0] * plane1.mD) / dir[1] ;
90                break;
91        case 2:
92                point[0] = (plane1.mNormal[1] * plane2.mD - plane2.mNormal[1] * plane1.mD) / dir[2] ;
93                point[1] = (plane2.mNormal[0] * plane1.mD - plane1.mNormal[0] * plane2.mD) / dir[2] ;
94                point[2] = 0.0f ;
95                break;
96        }
97
98        Normalize(dir);
99        return SimpleRay(point, dir, 0, 1.0f);
100}
101
102
103Vector3 Plane3::FindIntersection(const Vector3 &a,                                                 
104                                                                 const Vector3 &b,
105                                                                 float *t,
106                                                                 bool *coplanar) const
107{
108        //cout << "a: " << a << " b: " << b << endl;
109        const Vector3 v = b - a; // line from A to B
110        float dv = DotProd(v, mNormal);
111   
112    if (signum(dv) == 0)
113        {
114                if (coplanar)
115                {
116                        (*coplanar) = true;     
117                }
118
119                if (t)
120                {
121                        (*t) = 0;
122                }
123
124                return a;
125        }
126       
127        if (coplanar)
128        {
129                (*coplanar) = false;
130        }
131
132    float u = - Distance(a) / dv; // TODO: could be done more efficiently
133   
134        if (t)
135        {
136                (*t) = u;
137        }
138
139    return a + u * v;
140}
141
142
143int Plane3::Side(const Vector3 &v, const float threshold) const
144{
145    return signum(Distance(v), threshold);
146}
147
148
149float Plane3::FindT(const Vector3 &a, const Vector3 &b) const
150{         
151        const Vector3 v = b - a; // line from A to B
152        const float dv = DotProd(v, mNormal);
153   
154        // does not intersect or coincident
155        if (signum(dv) == 0)
156                return 0;
157       
158        return - Distance(a) / dv; // TODO: could be done more efficiently
159}
160
161
162float Plane3::FindT(const SimpleRay &a) const
163{         
164        const Vector3 v = a.mDirection; // line from A to B
165        const float dv = DotProd(v, mNormal);
166   
167        // does not intersect or coincident
168        if (signum(dv) == 0)
169                return 0;
170       
171        return - Distance(a.mOrigin) / dv; // TODO: could be done more efficiently
172}
173
174}
Note: See TracBrowser for help on using the repository browser.