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

Revision 1524, 3.9 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);
100}
101
102
103Vector3 Plane3::FindIntersection(const Vector3 &a,                                                 
104                                                                 const Vector3 &b,
105                                                                 float *t,
106                                                                 bool *coplanar) const
107{
108    const Vector3 v = b - a; // line from A to B
109    float dv = DotProd(v, mNormal);
110   
111    if (signum(dv) == 0)
112        {
113                if (coplanar)
114                {
115                        (*coplanar) = true;     
116                }
117
118                if (t)
119                {
120                        (*t) = 0;
121                }
122
123                return a;
124        }
125       
126        if (coplanar)
127        {
128                (*coplanar) = false;
129        }
130
131    float u = - Distance(a) / dv; // TODO: could be done more efficiently
132   
133        if (t)
134        {
135                (*t) = u;
136        }
137
138    return a + u * v;
139}
140
141
142int Plane3::Side(const Vector3 &v, const float threshold) const
143{
144    return signum(Distance(v), threshold);
145}
146
147
148float Plane3::FindT(const Vector3 &a, const Vector3 &b) const
149{         
150        const Vector3 v = b - a; // line from A to B
151        const float dv = DotProd(v, mNormal);
152   
153        // does not intersect or coincident
154        if (signum(dv) == 0)
155                return 0;
156       
157        return - Distance(a) / dv; // TODO: could be done more efficiently
158}
159
160
161float Plane3::FindT(const SimpleRay &a) const
162{         
163        const Vector3 v = a.mDirection; // line from A to B
164        const float dv = DotProd(v, mNormal);
165   
166        // does not intersect or coincident
167        if (signum(dv) == 0)
168                return 0;
169       
170        return - Distance(a.mOrigin) / dv; // TODO: could be done more efficiently
171}
172
173}
Note: See TracBrowser for help on using the repository browser.