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

Revision 256, 670 bytes checked in by bittner, 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}
Note: See TracBrowser for help on using the repository browser.