OPC_PlanesTriOverlap.h
Go to the documentation of this file.
1 #include "../CollisionData.h"
2 #include "../CollisionPairInserter.h"
4 
12 {
13  // Stats
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.0f && d1>0.0f && d2>0.0f) return FALSE;
27 // if(!(IR(d0)&SIGN_BITMASK) && !(IR(d1)&SIGN_BITMASK) && !(IR(d2)&SIGN_BITMASK)) return FALSE;
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 }
Vector3 CD_Trans1
translation of the first mesh
std::vector< collision_data > & collisions()
get collision information
int c
Definition: autoplay.py:16
double CD_s1
scale of the first mesh
udword mNbVolumePrimTests
Number of Volume-Primitive tests.
#define FALSE
Definition: OPC_IceHook.h:9
hrp::CollisionPairInserterBase * collisionPairInserter
VertexPointers mVP
#define TRUE
Definition: OPC_IceHook.h:13
#define inline_
const Point * Vertex[3]
Definition: IcePoint.h:25
Definition: IcePlane.h:17
inline_ BOOL PlanesTriOverlap(udword in_clip_mask)
Eigen::Vector3d Vector3
Definition: EigenTypes.h:11
int BOOL
Another boolean type.
Definition: IceTypes.h:102
unsigned int udword
sizeof(udword) must be 4
Definition: IceTypes.h:65
Point n
The normal to the plane.
Definition: IcePlane.h:53
inline_ float Distance(const Point &p) const
Definition: IcePlane.h:40
void getVector3(Vector3 &v3, const V &v, size_t top=0)
Definition: Eigen3d.h:138
Matrix33 CD_Rot1
rotation of the first mesh


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat Apr 13 2019 02:14:24