fLineVec.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * The University of Tokyo
00008  */
00009 /*
00010  * fLineVec.h
00011  * Create: Katsu Yamane, 03.04.11
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 


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:53