IceSegment.cpp
Go to the documentation of this file.
00001 
00002 
00008 
00009 
00011 
00021 
00022 
00024 // Precompiled Header
00025 #include "Stdafx.h"
00026 
00027 using namespace IceMaths;
00028 
00029 float Segment::SquareDistance(const Point& point, float* t)     const
00030 {
00031         Point Diff = point - mP0;
00032         Point Dir = mP1 - mP0;
00033         float fT = Diff | Dir;
00034 
00035         if(fT<=0.0f)
00036         {
00037                 fT = 0.0f;
00038         }
00039         else
00040         {
00041                 float SqrLen= Dir.SquareMagnitude();
00042                 if(fT>=SqrLen)
00043                 {
00044                         fT = 1.0f;
00045                         Diff -= Dir;
00046                 }
00047                 else
00048                 {
00049                         fT /= SqrLen;
00050                         Diff -= fT*Dir;
00051                 }
00052         }
00053 
00054         if(t)   *t = fT;
00055 
00056         return Diff.SquareMagnitude();
00057 }


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:54