hrplib
hrpCollision
Opcode
OPC_PlanesTriOverlap.h
Go to the documentation of this file.
1
#include "../CollisionData.h"
2
#include "../CollisionPairInserter.h"
4
10
inline_
BOOL
PlanesCollider::PlanesTriOverlap
(
udword
in_clip_mask)
12
{
13
// Stats
14
mNbVolumePrimTests
++;
15
16
const
Plane
* p =
mPlanes
;
17
udword
Mask = 1;
18
19
while
(Mask<=in_clip_mask)
20
{
21
if
(in_clip_mask & Mask)
22
{
23
float
d0 = p->
Distance
(*
mVP
.
Vertex
[0]);
24
float
d1 = p->
Distance
(*
mVP
.
Vertex
[1]);
25
float
d2 = p->
Distance
(*
mVP
.
Vertex
[2]);
26
if
(d0>0.0
f
&& d1>0.0
f
&& d2>0.0
f
)
return
FALSE
;
27
// if(!(IR(d0)&SIGN_BITMASK) && !(IR(d1)&SIGN_BITMASK) && !(IR(d2)&SIGN_BITMASK)) return FALSE;
28
if
(
collisionPairInserter
){
29
hrp::CollisionPairInserterBase
&
c
= *
collisionPairInserter
;
30
hrp::collision_data
cdata;
31
cdata.
num_of_i_points
= 2;
32
cdata.
i_point_new
[0] = cdata.
i_point_new
[1] = 1;
33
cdata.
i_point_new
[2] = cdata.
i_point_new
[3] = 0;
34
hrp::Vector3
relN;
35
hrp::getVector3
(relN, p->
n
);
36
cdata.
n_vector
=
c
.CD_Rot1*relN;
37
Point
p1, p2;
38
if
(d0<=0 && d1>0 && d2>0){
39
cdata.
depth
= -d0;
40
p1 = ((*
mVP
.
Vertex
[0])*d1-(*
mVP
.
Vertex
[1])*d0)/(d1-d0);
41
p2 = ((*
mVP
.
Vertex
[0])*d2-(*
mVP
.
Vertex
[2])*d0)/(d2-d0);
42
}
else
if
(d0>0 && d1<=0 && d2>0){
43
cdata.
depth
= -d1;
44
p1 = ((*
mVP
.
Vertex
[1])*d0-(*
mVP
.
Vertex
[0])*d1)/(d0-d1);
45
p2 = ((*
mVP
.
Vertex
[1])*d2-(*
mVP
.
Vertex
[2])*d1)/(d2-d1);
46
}
else
if
(d0>0 && d1>0 && d2<=0){
47
cdata.
depth
= -d2;
48
p1 = ((*
mVP
.
Vertex
[2])*d0-(*
mVP
.
Vertex
[0])*d2)/(d0-d2);
49
p2 = ((*
mVP
.
Vertex
[2])*d1-(*
mVP
.
Vertex
[1])*d2)/(d1-d2);
50
}
else
if
(d0<=0 && d1<=0 && d2>0){
51
cdata.
depth
= d0 > d1 ? -d1 : -d0;
52
p1 = ((*
mVP
.
Vertex
[0])*d2-(*
mVP
.
Vertex
[2])*d0)/(d2-d0);
53
p2 = ((*
mVP
.
Vertex
[1])*d2-(*
mVP
.
Vertex
[2])*d1)/(d2-d1);
54
}
else
if
(d0<=0 && d1>0 && d2<=0){
55
cdata.
depth
= d0 > d2 ? -d2 : -d0;
56
p1 = ((*
mVP
.
Vertex
[0])*d1-(*
mVP
.
Vertex
[1])*d0)/(d1-d0);
57
p2 = ((*
mVP
.
Vertex
[2])*d1-(*
mVP
.
Vertex
[1])*d2)/(d1-d2);
58
}
else
if
(d0>0 && d1<=0 && d2<=0){
59
cdata.
depth
= d1 > d2 ? -d2 : -d1;
60
p1 = ((*
mVP
.
Vertex
[1])*d0-(*
mVP
.
Vertex
[0])*d1)/(d0-d1);
61
p2 = ((*
mVP
.
Vertex
[2])*d0-(*
mVP
.
Vertex
[0])*d2)/(d0-d2);
62
}
63
if
(d0>0||d1>0||d2>0){
64
hrp::Vector3
v1, v2;
65
hrp::getVector3
(v1, p1);
66
hrp::getVector3
(v2, p2);
67
cdata.
i_points
[0] =
c
.CD_s1 * (
c
.CD_Rot1 * v1 +
c
.CD_Trans1);
68
cdata.
i_points
[1] =
c
.CD_s1 * (
c
.CD_Rot1 * v2 +
c
.CD_Trans1);
69
collisionPairInserter
->
collisions
().push_back(cdata);
70
}
71
}
72
}
73
Mask+=Mask;
74
p++;
75
}
76
/*
77
for(udword i=0;i<6;i++)
78
{
79
float d0 = p[i].Distance(mLeafVerts[0]);
80
float d1 = p[i].Distance(mLeafVerts[1]);
81
float d2 = p[i].Distance(mLeafVerts[2]);
82
if(d0>0.0f && d1>0.0f && d2>0.0f) return false;
83
}
84
*/
85
return
TRUE
;
86
}
hrp::collision_data::num_of_i_points
int num_of_i_points
Definition:
CollisionData.h:26
hrp::collision_data::depth
double depth
Definition:
CollisionData.h:31
hrp::collision_data::n_vector
Vector3 n_vector
Definition:
CollisionData.h:30
udword
unsigned int udword
sizeof(udword) must be 4
Definition:
IceTypes.h:65
Plane::Distance
inline_ float Distance(const Point &p) const
Definition:
IcePlane.h:40
BOOL
int BOOL
Another boolean type.
Definition:
IceTypes.h:102
swingTest.f
f
Definition:
swingTest.py:6
PlanesCollider::mVP
VertexPointers mVP
Definition:
OPC_PlanesCollider.h:96
Plane::n
Point n
The normal to the plane.
Definition:
IcePlane.h:53
hrp::Vector3
Eigen::Vector3d Vector3
Definition:
EigenTypes.h:11
VolumeCollider::mNbVolumePrimTests
udword mNbVolumePrimTests
Number of Volume-Primitive tests.
Definition:
OPC_VolumeCollider.h:100
VertexPointers::Vertex
const Point * Vertex[3]
Definition:
OPC_MeshInterface.h:25
hrp::collision_data
Definition:
CollisionData.h:20
PlanesCollider::PlanesTriOverlap
inline_ BOOL PlanesTriOverlap(udword in_clip_mask)
Definition:
OPC_PlanesTriOverlap.h:11
PlanesCollider::collisionPairInserter
hrp::CollisionPairInserterBase * collisionPairInserter
Definition:
OPC_PlanesCollider.h:91
TRUE
#define TRUE
Definition:
OPC_IceHook.h:13
autoplay.c
int c
Definition:
autoplay.py:16
hrp::getVector3
void getVector3(Vector3 &v3, const V &v, size_t top=0)
Definition:
Eigen3d.h:138
FALSE
#define FALSE
Definition:
OPC_IceHook.h:9
Point
Definition:
IcePoint.h:25
Plane
Definition:
IcePlane.h:17
hrp::CollisionPairInserterBase::collisions
std::vector< collision_data > & collisions()
get collision information
Definition:
CollisionPairInserterBase.h:95
hrp::collision_data::i_points
Vector3 i_points[4]
Definition:
CollisionData.h:27
hrp::CollisionPairInserterBase
Definition:
CollisionPairInserterBase.h:27
hrp::collision_data::i_point_new
int i_point_new[4]
Definition:
CollisionData.h:28
inline_
#define inline_
Definition:
IcePreprocessor.h:103
PlanesCollider::mPlanes
Plane * mPlanes
Definition:
OPC_PlanesCollider.h:94
openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Wed Sep 7 2022 02:51:03