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

Revision 2575, 4.0 KB checked in by bittner, 17 years ago (diff)

big merge: preparation for havran ray caster, check if everything works

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