hrplib
hrpCollision
Opcode
Ice
IceRay.cpp
Go to the documentation of this file.
1
8
11
18
20
/*
21
O = Origin = impact point
22
i = normalized vector along the x axis
23
j = normalized vector along the y axis = actually the normal vector in O
24
D = Direction vector, norm |D| = 1
25
N = Projection of D on y axis, norm |N| = normal reaction
26
T = Projection of D on x axis, norm |T| = tangential reaction
27
R = Reflexion vector
28
29
^y
30
|
31
|
32
|
33
_ _ _| _ _ _
34
* * *|
35
\ | /
36
\ |N / |
37
R\ | /D
38
\ | / |
39
\ | /
40
_________\|/______*_______>x
41
O T
42
43
Let define theta = angle between D and N. Then cos(theta) = |N| / |D| = |N| since D is normalized.
44
45
j|D = |j|*|D|*cos(theta) => |N| = j|D
46
47
Then we simply have:
48
49
D = N + T
50
51
To compute tangential reaction :
52
53
T = D - N
54
55
To compute reflexion vector :
56
57
R = N - T = N - (D-N) = 2*N - D
58
*/
59
61
// Precompiled Header
62
#include "
Stdafx.h
"
63
64
using namespace
IceMaths
;
65
66
float
Ray::SquareDistance
(
const
Point
& point,
float
* t)
const
67
{
68
Point
Diff = point -
mOrig
;
69
float
fT = Diff |
mDir
;
70
71
if
(fT<=0.0
f
)
72
{
73
fT = 0.0f;
74
}
75
else
76
{
77
fT /=
mDir
.
SquareMagnitude
();
78
Diff -= fT*
mDir
;
79
}
80
81
if
(t) *t = fT;
82
83
return
Diff.
SquareMagnitude
();
84
}
IceMaths::Ray::SquareDistance
float SquareDistance(const Point &point, float *t=null) const
Definition:
IceRay.cpp:66
IceMaths::Point::SquareMagnitude
inline_ float SquareMagnitude() const
Computes square magnitude.
Definition:
OPC_IceHook.h:218
IceMaths::Point
Definition:
OPC_IceHook.h:26
Stdafx.h
IceMaths::Ray::mOrig
Point mOrig
Ray origin.
Definition:
OPC_IceHook.h:31
IceMaths
Definition:
ColdetModel.h:24
IceMaths::Ray::mDir
Point mDir
Normalized direction.
Definition:
OPC_IceHook.h:32
swingTest.f
f
Definition:
swingTest.py:6
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