Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014 #include "fLineVec.h"
00015
00016 ostream& operator << (ostream& ost, fLineVec& v)
00017 {
00018 ost << v.v_org << v.v_dir << flush;
00019 return ost;
00020 }
00021
00022 fVec3 fLineVec::position(double t) const
00023 {
00024 fVec3 p;
00025 p.mul(v_dir, t);
00026 p += v_org;
00027 return p;
00028 }
00029
00030 void fLineVec::position(double t, fVec3& p) const
00031 {
00032 p.mul(v_dir, t);
00033 p += v_org;
00034 }
00035
00036 int intersection(const fLineVec& lv1, const fLineVec& lv2,
00037 fVec3& c1, fVec3& c2, double& d, double eps)
00038 {
00039 int parallel = false;
00040 fVec3 p1(lv1.Org()), d1(lv1.Dir());
00041 fVec3 p2(lv2.Org()), d2(lv2.Dir());
00042 double dd11, dd12, dd22;
00043 double dp11, dp12, dp21, dp22;
00044 double f, g1, g2;
00045 double t1, t2;
00046 dd11 = d1 * d1;
00047 dd12 = d1 * d2;
00048 dd22 = d2 * d2;
00049 dp11 = d1 * p1;
00050 dp12 = d1 * p2;
00051 dp21 = d2 * p1;
00052 dp22 = d2 * p2;
00053 f = dd11*dd22 - dd12*dd12;
00054 g1 = dp12 - dp11;
00055 g2 = dp22 - dp21;
00056 if(fabs(f) < eps)
00057 {
00058 t1 = g1 / dd11;
00059 t2 = 0.0;
00060 parallel = true;
00061 }
00062 else
00063 {
00064 t1 = (dd22*g1 - dd12*g2) / f;
00065 t2 = (dd12*g1 - dd11*g2) / f;
00066 }
00067 lv1.position(t1, c1);
00068 lv2.position(t2, c2);
00069 d = dist(c1, c2);
00070 return parallel;
00071 }
00072
00073 double fLineVec::distance(const fVec3& point, fVec3& pos, double* k)
00074 {
00075 static fVec3 pp;
00076 double d2, dp, t;
00077 pp.sub(point, v_org);
00078 d2 = v_dir * v_dir;
00079 dp = v_dir * pp;
00080 t = dp / d2;
00081 pos.mul(t, v_dir);
00082 pos += v_org;
00083 pp.sub(point, pos);
00084 if(k) *k = t;
00085 return pp.length();
00086 }
00087