fLineVec.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * The University of Tokyo
8  */
9 /*
10  * fLineVec.h
11  * Create: Katsu Yamane, 03.04.11
12  */
13 
14 #include "fLineVec.h"
15 
16 ostream& operator << (ostream& ost, fLineVec& v)
17 {
18  ost << v.v_org << v.v_dir << flush;
19  return ost;
20 }
21 
22 fVec3 fLineVec::position(double t) const
23 {
24  fVec3 p;
25  p.mul(v_dir, t);
26  p += v_org;
27  return p;
28 }
29 
30 void fLineVec::position(double t, fVec3& p) const
31 {
32  p.mul(v_dir, t);
33  p += v_org;
34 }
35 
36 int intersection(const fLineVec& lv1, const fLineVec& lv2,
37  fVec3& c1, fVec3& c2, double& d, double eps)
38 {
39  int parallel = false;
40  fVec3 p1(lv1.Org()), d1(lv1.Dir());
41  fVec3 p2(lv2.Org()), d2(lv2.Dir());
42  double dd11, dd12, dd22;
43  double dp11, dp12, dp21, dp22;
44  double f, g1, g2;
45  double t1, t2;
46  dd11 = d1 * d1;
47  dd12 = d1 * d2;
48  dd22 = d2 * d2;
49  dp11 = d1 * p1;
50  dp12 = d1 * p2;
51  dp21 = d2 * p1;
52  dp22 = d2 * p2;
53  f = dd11*dd22 - dd12*dd12;
54  g1 = dp12 - dp11;
55  g2 = dp22 - dp21;
56  if(fabs(f) < eps)
57  {
58  t1 = g1 / dd11;
59  t2 = 0.0;
60  parallel = true;
61  }
62  else
63  {
64  t1 = (dd22*g1 - dd12*g2) / f;
65  t2 = (dd12*g1 - dd11*g2) / f;
66  }
67  lv1.position(t1, c1);
68  lv2.position(t2, c2);
69  d = dist(c1, c2);
70  return parallel;
71 }
72 
73 double fLineVec::distance(const fVec3& point, fVec3& pos, double* k)
74 {
75  static fVec3 pp;
76  double d2, dp, t;
77  pp.sub(point, v_org);
78  d2 = v_dir * v_dir;
79  dp = v_dir * pp;
80  t = dp / d2;
81  pos.mul(t, v_dir);
82  pos += v_org;
83  pp.sub(point, pos);
84  if(k) *k = t;
85  return pp.length();
86 }
87 
fVec3 v_dir
Definition: fLineVec.h:127
friend fVec3 & Org(fLineVec &v)
Definition: fLineVec.h:62
friend int intersection(const fLineVec &lv1, const fLineVec &lv2, fVec3 &c1, fVec3 &c2, double &d, double eps)
Definition: fLineVec.cpp:36
t
double distance(const fVec3 &point, fVec3 &pos, double *k=0)
Definition: fLineVec.cpp:73
ostream & operator<<(ostream &ost, fLineVec &v)
Definition: fLineVec.cpp:16
void sub(const fVec3 &vec1, const fVec3 &vec2)
Definition: fMatrix3.cpp:902
3-element vector class.
Definition: fMatrix3.h:206
fVec3 v_org
Definition: fLineVec.h:126
friend fVec3 & Dir(fLineVec &v)
Definition: fLineVec.h:65
void mul(const fVec3 &vec, double d)
Definition: fMatrix3.cpp:916
#define eps
Definition: fEulerPara.cpp:17
fVec3 position(double t) const
Definition: fLineVec.cpp:22


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:03