IceRay.cpp
Go to the documentation of this file.
00001 
00002 
00008 
00009 
00011 
00018 
00019 
00020 /*
00021         O = Origin = impact point
00022         i = normalized vector along the x axis
00023         j = normalized vector along the y axis = actually the normal vector in O
00024         D = Direction vector, norm |D| = 1
00025         N = Projection of D on y axis, norm |N| = normal reaction
00026         T = Projection of D on x axis, norm |T| = tangential reaction
00027         R = Reflexion vector
00028 
00029               ^y
00030               |
00031               |
00032               |
00033        _  _  _| _ _ _
00034        *      *      *|
00035         \     |     /
00036          \    |N   /  |
00037          R\   |   /D
00038            \  |  /    |
00039             \ | /
00040     _________\|/______*_______>x
00041                O    T
00042 
00043         Let define theta = angle between D and N. Then cos(theta) = |N| / |D| = |N| since D is normalized.
00044 
00045         j|D = |j|*|D|*cos(theta) => |N| = j|D
00046 
00047         Then we simply have:
00048 
00049         D = N + T
00050 
00051         To compute tangential reaction :
00052 
00053         T = D - N
00054 
00055         To compute reflexion vector :
00056 
00057         R = N - T = N - (D-N) = 2*N - D
00058 */
00059 
00061 // Precompiled Header
00062 #include "Stdafx.h"
00063 
00064 using namespace IceMaths;
00065 
00066 float Ray::SquareDistance(const Point& point, float* t) const
00067 {
00068         Point Diff = point - mOrig;
00069         float fT = Diff | mDir;
00070 
00071         if(fT<=0.0f)
00072         {
00073                 fT = 0.0f;
00074         }
00075         else
00076         {
00077                 fT /= mDir.SquareMagnitude();
00078                 Diff -= fT*mDir;
00079         }
00080 
00081         if(t) *t = fT;
00082 
00083         return Diff.SquareMagnitude();
00084 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Apr 11 2019 03:30:17