void R3MeshSearchTree::
FindIntersection(const R3Ray& ray, R3MeshIntersection& closest,
RNScalar min_t, RNScalar max_t,
int (*IsCompatible)(const R3Point&, const R3Vector&, R3Mesh *, R3MeshFace *, void *), void *compatible_data)
{
// Initialize result
closest.type = R3_MESH_NULL_TYPE;
closest.vertex = NULL;
closest.edge = NULL;
closest.face = NULL;
closest.point = R3zero_point;
closest.t = 0;
// Check root
if (!root) return;
// Update mark (used to avoid checking same face twice)
mark++;
// Search nodes recursively
FindIntersection(ray, closest,
min_t, max_t,
IsCompatible, compatible_data,
root, BBox());
}
//----------------------------------------------------------------------------
void TestTriTri::GetIntersecting ()
{
Vector2 akV0[3], akV1[3];
for (int i = 0; i < 3; i++)
{
akV0[i].x = (Real) m_aiX0[i];
akV0[i].y = (Real) m_aiY0[i];
akV1[i].x = (Real) m_aiX1[i];
akV1[i].y = (Real) m_aiY1[i];
}
Vector2 kW0, kW1;
Real fTLast, fTMax = 1.0;
m_iQuantity = 0;
switch ( m_eType )
{
case TT_TEST:
m_bIntersecting = TestIntersection(akV0,akV1);
break;
case TT_FIND:
m_bIntersecting = FindIntersection(akV0,akV1,m_iQuantity,
m_akVertex);
break;
case TT_TEST_VEL:
kW0 = GetVelocity(m_uiSpeed0,m_uiAngle0);
kW1 = GetVelocity(m_uiSpeed1,m_uiAngle1);
m_bIntersecting = TestIntersection(fTMax,akV0,kW0,akV1,kW1,
m_fTFirst,fTLast);
break;
case TT_FIND_VEL:
kW0 = GetVelocity(m_uiSpeed0,m_uiAngle0);
kW1 = GetVelocity(m_uiSpeed1,m_uiAngle1);
m_bIntersecting = FindIntersection(fTMax,akV0,kW0,akV1,kW1,
m_fTFirst,fTLast,m_iQuantity,m_akVertex);
break;
}
InvalidateRect(GetWindowHandle(),NULL,TRUE);
}
void ClipLine(ClipVertex* a, ClipVertex* b, const ViewState& s, int plane)
// Clips the line defined by the given vertices against the clipping
// plane of the given view state indexed by the 'plane' parameter. Recurses
// on the next clipping plane, if there is one. When it runs out of clip
// planes, it passes the line to DrawUnclippedLine().
{
// Check to see if we're done clipping.
if (plane >= s.ClipPlaneCount) {
DrawUnclippedLine(a, b, s);
return;
}
// Figure out which side of the clipping plane each of the vertices is on.
int mask = 1 << plane;
bool aclip, bclip;
aclip = a->GetOutcode() & mask ? true : false;
bclip = b->GetOutcode() & mask ? true : false;
int clipcount = int(aclip) + int(bclip);
// See if all the vertices are on the right side of the clipping plane.
if (clipcount == 0) {
// No clipping against this plane is needed.
ClipLine(a, b, s, plane + 1);
return;
}
// See if all the vertices are on the wrong side of the clipping plane.
if (clipcount == 2) {
// line is not visible.
return;
}
// Sort so that a is on the right side.
if (aclip) {
ClipVertex* temp = a; a = b; b = temp;
}
// Convenience variables.
const vec3& Normal = s.ClipPlane[plane].Normal;
float D = s.ClipPlane[plane].D;
// Find the vertex at the intersection of clipped line.
ClipVertex c;
FindIntersection(&c, *a, *b, Normal, D);
c.Preprocess(s);
// Draw the visible fragment.
ClipLine(a, &c, s, plane + 1);
}
// Use the list of polygons to generate the relevant impedances in the TLM grid
void AddPolygonsToGrid(PolygonGroup *Head)
{
PolygonGroup *PolygonGroupPtr;
Polygon *PolygonPtr;
PolygonGroupPtr = Head;
while (PolygonGroupPtr != NULL) {
PolygonPtr = PolygonGroupPtr->PolygonList;
if (PolygonGroupPtr->Type == Vertical) {
// Add vertical polygons into the grid
while (PolygonPtr != NULL) {
// Check all polygons with a lower priority for intersections
PolygonGroup *IntersectionTestGroup = Head;
Polygon *IntersectionTest;
// Ensure only vertical polygons of lower priority and greater thickness are checked
while (IntersectionTestGroup != NULL && IntersectionTestGroup->Type == Vertical && IntersectionTestGroup->Priority < PolygonGroupPtr->Priority && IntersectionTestGroup->Thickness > PolygonGroupPtr->Thickness) {
IntersectionTest = IntersectionTestGroup->PolygonList;
while(IntersectionTest != NULL) {
// Find the intersection of the two polygons, if there is one
Polygon *Intersection;
Intersection = FindIntersection(IntersectionTest, PolygonPtr, IntersectionTestGroup->Thickness);
if (Intersection != NULL) {
// Add an air gap the size of the intersection to the grid
AddVerticalPolygon(Intersection, IntersectionTestGroup->Thickness, 1.0, true);
free(Intersection);
}
IntersectionTest = IntersectionTest->NextPolygon;
}
IntersectionTestGroup = IntersectionTestGroup->NextPolygonGroup;
}
AddVerticalPolygon(PolygonPtr, PolygonGroupPtr->Thickness, PolygonGroupPtr->Permittivity, PolygonGroupPtr->PropagateFlag);
PolygonPtr = PolygonPtr->NextPolygon;
}
}
else {
// Add horizontal polygons into the grid
while (PolygonPtr != NULL) {
AddHorizontalPolygon(PolygonPtr, PolygonGroupPtr->Thickness, PolygonGroupPtr->Permittivity, PolygonGroupPtr->PropagateFlag);
PolygonPtr = PolygonPtr->NextPolygon;
}
}
PolygonGroupPtr = PolygonGroupPtr->NextPolygonGroup;
}
}
/// split the fractures if they intersect
void TPZFracSet::ComputeFractureIntersections()
{
int64_t ifr=0;
while (ifr < fFractureVec.NElements()) {
int64_t jfr = ifr+1;
int64_t nel = fFractureVec.NElements();
double x0,x1,y0,y1;
int64_t inode0 = fFractureVec[ifr].fNodes[0];
int64_t inode1 = fFractureVec[ifr].fNodes[1];
x0 = fNodeVec[inode0].Coord(0);
y0 = fNodeVec[inode0].Coord(1);
x1 = fNodeVec[inode1].Coord(0);
y1 = fNodeVec[inode1].Coord(1);
while (jfr < nel) {
double a0,a1,b0,b1,xy,ab;
int64_t jnode0 = fFractureVec[jfr].fNodes[0];
int64_t jnode1 = fFractureVec[jfr].fNodes[1];
a0 = fNodeVec[jnode0].Coord(0);
b0 = fNodeVec[jnode0].Coord(1);
a1 = fNodeVec[jnode1].Coord(0);
b1 = fNodeVec[jnode1].Coord(1);
if(FindIntersection(x0, y0, x1, y1, a0, b0, a1, b1, xy, ab) && ((xy > 0.01 && xy < 0.99) || (ab > 0.01 && ab < 0.99)))
{
SplitFractures(ifr, xy, jfr, ab);
// the fracture ifr has changed!
inode0 = fFractureVec[ifr].fNodes[0];
inode1 = fFractureVec[ifr].fNodes[1];
x0 = fNodeVec[inode0].Coord(0);
y0 = fNodeVec[inode0].Coord(1);
x1 = fNodeVec[inode1].Coord(0);
y1 = fNodeVec[inode1].Coord(1);
}
jfr++;
}
ifr++;
}
}
void SplitPolygon(std::vector<pcs_polygon> &polygons, int polynum, vector3d plane_point, vector3d plane_normal, std::vector<pcs_polygon> &newpolys)
{
std::vector<pcs_polygon> splitpolys(2); // 0 = front vert, 1 = back vert
std::vector<int> pairs;
std::vector<pcs_vertex> points;
pairs.resize(polygons[polynum].verts.size() * 2);
pcs_vertex temp;
unsigned int i, j = 0;
float uvdelta;
for (i = 0; i < polygons[polynum].verts.size() * 2; i += 2)
{
pairs[i] = j % polygons[polynum].verts.size();
pairs[i+1] = (j+1) % polygons[polynum].verts.size();
j++;
}
float dtempa, dtempb;
// compile the new list of points
for (i = 0; i < pairs.size(); i += 2)
{
dtempa = DistanceToPlane(polygons[polynum].verts[pairs[i]].point, plane_point, plane_normal);
dtempb = DistanceToPlane(polygons[polynum].verts[pairs[i+1]].point, plane_point, plane_normal);
if ((dtempa <= 0.0001 && dtempa >= -0.0001) || (dtempb <= 0.0001 && dtempb >= -0.0001))
// one of these points lays on the plane... add them both without modification
{
AddIfNotInList(points, polygons[polynum].verts[pairs[i]]);
AddIfNotInList(points, polygons[polynum].verts[pairs[i+1]]);
}
else // neither of them was not on the plane.. are they on the same side?
{
if (InFrontofPlane(polygons[polynum].verts[pairs[i]].point, plane_point, plane_normal) ==
InFrontofPlane(polygons[polynum].verts[pairs[i+1]].point, plane_point, plane_normal))
// both on same side - add them
{
AddIfNotInList(points, polygons[polynum].verts[pairs[i]]);
AddIfNotInList(points, polygons[polynum].verts[pairs[i+1]]);
}
else
// different sides - cut and add
{
uvdelta = FindIntersection(temp.point, polygons[polynum].verts[pairs[i]].point,
polygons[polynum].verts[pairs[i+1]].point, plane_point, plane_normal);
temp.norm = polygons[polynum].norm;
temp.u = uvdelta * (polygons[polynum].verts[pairs[i]].u - polygons[polynum].verts[pairs[i+1]].u);
temp.v = uvdelta * (polygons[polynum].verts[pairs[i]].v - polygons[polynum].verts[pairs[i+1]].v);
AddIfNotInList(points, polygons[polynum].verts[pairs[i]]);
AddIfNotInList(points, temp);
AddIfNotInList(points, polygons[polynum].verts[pairs[i+1]]);
}
}
}
// split the polygons with the list we have
int in = 0;
for (i = 0; i < points.size(); i++)
{
dtempa = DistanceToPlane(points[i].point, plane_point, plane_normal) ;
if (dtempa <= 0.0001 && dtempa >= -0.0001)
// there WILL be two points in the list this is true for
{
AddIfNotInList(splitpolys[0].verts, points[i]);
AddIfNotInList(splitpolys[1].verts, points[i]);
if (in == 0)
in = 1;
else
in = 0;
}
else
{
AddIfNotInList(splitpolys[in].verts, points[i]);
}
}
// triangle our new polylist
TriangulateMesh(splitpolys);
// replace our current poly with polygon zero - add the others
polygons[polynum] = splitpolys[0];
in = newpolys.size();
newpolys.resize(in+splitpolys.size());
for (i = 1; i < splitpolys.size(); i++)
{
newpolys[in+i] = splitpolys[i];
}
}
int main(int argc, char* argv[]){
// Declare variables-------------------------------------------------------------
HashTable Index; // Inverted index
InitialiseHashTable(&Index);
char text[MAXLEN];
int test = 0;
// 1. Check input parameters--------------------------------------------------------
if (argc != 3 ){ // check number of arguments
fprintf(stderr,"Error: Incorrect number of input argument\n");
return -1;
}else if(!IsFile(argv[1])){ // check if file is valid
fprintf(stderr,"Error: File %s is invalid\n", argv[1]);
return -1;
}else if(!IsDir(argv[2])){ // check if directory is valid
fprintf(stderr,"Error: Directory %s cannot be found\n", argv[2]);
return -1;
}
// 2. Reconstruct Inverted Index-----------------------------------------------------
printf("Please wait while the query engine is loading. It might take a few minutes... \n");
if(!ReadFile(&Index, argv[1])){
CleanUpHash(&Index);
return -1;
}
// 3. Command Line interface and query -----------------------------------------------
for(int j=0; j<9; j++){
// Create text array for automated testing
switch (j){
case 0:
printf("\n3.%d Test invalid input syntax\n",j+1);
printf("QUERY :> AND dog\n");
strcpy(text,"AND dog\n");
break;
case 1:
printf("\n3.%d Test invalid input syntax\n", j+1);
printf("QUERY :> cat OR AND dog\n");
strcpy(text,"cat OR AND dog\n");
break;
case 2:
printf("\n3.%d Test no result\n", j+1);
printf("QUERY :> thisisrandom\n");
strcpy(text,"thisisrandom\n");
break;
case 3:
printf("\n3.%d Test single entry\n", j+1);
printf("QUERY :> incredible\n");
strcpy(text,"incredible\n");
break;
case 4:
printf("\n3.%d Test uppercase\n", j+1);
printf("QUERY :> Incredible\n");
strcpy(text,"Incredible\n");
break;
case 5:
printf("\n3.%d Test AND\n", j+1);
printf("QUERY :> Dartmouth AND College AND Computer AND Science\n");
strcpy(text,"Dartmouth AND College AND Computer AND Science\n");
break;
case 6:
printf("\n3.%d Test space as AND\n", j+1);
printf("QUERY :> Dartmouth College Computer Science\n");
strcpy(text,"Dartmouth College Computer Science\n");
break;
case 7:
printf("\n3.%d Test OR\n", j+1);
printf("QUERY :> Dartmouth OR Computer\n");
strcpy(text,"Dartmouth OR Computer\n");
break;
case 8:
printf("\n3.%d Test combined\n", j+1);
printf("QUERY :> Dartmouth College AND Hanlon OR Mathematics AND Computer Science AND Philosophy OR incredibles Pixar\n");
strcpy(text,"Dartmouth College AND Hanlon OR Mathematics AND Computer Science AND Philosophy OR incredibles Pixar\n");
break;
}
// a) declare variables
int unionflag, flag, size_temp, size_intersect, size_final, count;
char wordarray[MAXLEN][MAXLEN];
int temparray[MAXSIZE][2], intersect[MAXSIZE][2], final[MAXSIZE][2];
// b) instantiate variables
size_temp = size_intersect = size_final = unionflag = flag = 0;
count = StringToWord(wordarray,text);
// c) query
for(int i=0; i<count; i++){
if(i==0 && strcmp(wordarray[i],"AND") && strcmp(wordarray[i],"OR")){ // if it's the first word and is not invalid
NormalizeWord(wordarray[i]);
size_intersect = FindHash(wordarray[i], intersect, Index);
continue;
}else if(i==0){ // if it is first word and invalid
flag = 1; break;
}else if(unionflag){
if(strcmp(wordarray[i],"AND") && strcmp(wordarray[i],"OR")){
NormalizeWord(wordarray[i]);
size_intersect = FindHash(wordarray[i], intersect, Index);
unionflag = 0;
continue;
}else{
flag = 1; break;
}
}
if (!strcmp(wordarray[i],"AND")){ // if it's AND
if(CheckOperator(wordarray,i,count)){
NormalizeWord(wordarray[i+1]);
size_temp = FindHash(wordarray[i+1], temparray, Index);
size_intersect = FindIntersection(intersect, size_intersect, temparray, size_temp);
i++;
continue;
}else{
flag = 1; break;
}
}else if(!strcmp(wordarray[i],"OR")){ // if it's OR
if(CheckOperator(wordarray,i,count)){
size_final = FindUnion(final, size_final, intersect, size_intersect);
size_intersect = 0;
unionflag = 1;
continue;
}else{
flag = 1; break;
}
}else{
NormalizeWord(wordarray[i]);
size_temp = FindHash(wordarray[i], temparray, Index);
size_intersect = FindIntersection(intersect, size_intersect, temparray, size_temp);
continue;
}
}
void R3MeshSearchTree::
FindIntersection(const R3Ray& ray, R3MeshIntersection& closest,
RNScalar min_t, RNScalar& max_t,
int (*IsCompatible)(const R3Point&, const R3Vector&, R3Mesh *, R3MeshFace *, void *), void *compatible_data,
R3MeshSearchTreeNode *node, const R3Box& node_box) const
{
// Find intersection with bounding box
RNScalar node_box_t;
if (!R3Intersects(ray, node_box, NULL, NULL, &node_box_t)) return;
if (node_box_t > max_t && !R3Contains(node_box, ray.Start())) return;
// Update based on closest intersection to each big face
for (int i = 0; i < node->big_faces.NEntries(); i++) {
// Get face container and check mark
R3MeshSearchTreeFace *face_container = node->big_faces[i];
if (face_container->mark == mark) continue;
face_container->mark = mark;
// Find closest point in mesh face
FindIntersection(ray, closest, min_t, max_t,
IsCompatible, compatible_data, face_container->face);
}
// Check if node is interior
if (node->children[0]) {
assert(node->children[1]);
assert(node->small_faces.IsEmpty());
// Compute distance from query point to split plane
RNScalar side = ray.Start()[node->split_dimension] - node->split_coordinate;
RNScalar vec = ray.Vector()[node->split_dimension];
RNScalar plane_t = (side*vec < 0) ? -side/vec : RN_INFINITY;
// Search children nodes
if (side <= 0) {
// Search negative side first
if (plane_t >= min_t) {
R3Box child_box(node_box);
child_box[RN_HI][node->split_dimension] = node->split_coordinate;
FindIntersection(ray, closest, min_t, max_t,
IsCompatible, compatible_data, node->children[0], child_box);
}
if (plane_t < max_t) {
R3Box child_box(node_box);
child_box[RN_LO][node->split_dimension] = node->split_coordinate;
FindIntersection(ray, closest, min_t, max_t,
IsCompatible, compatible_data, node->children[1], child_box);
}
}
else {
// Search positive side first
if (plane_t >= min_t) {
R3Box child_box(node_box);
child_box[RN_LO][node->split_dimension] = node->split_coordinate;
FindIntersection(ray, closest, min_t, max_t,
IsCompatible, compatible_data, node->children[1], child_box);
}
if (plane_t < max_t) {
R3Box child_box(node_box);
child_box[RN_HI][node->split_dimension] = node->split_coordinate;
FindIntersection(ray, closest, min_t, max_t,
IsCompatible, compatible_data, node->children[0], child_box);
}
}
}
else {
// Update based on distance to each small face
for (int i = 0; i < node->small_faces.NEntries(); i++) {
// Get face container and check mark
R3MeshSearchTreeFace *face_container = node->small_faces[i];
if (face_container->mark == mark) continue;
face_container->mark = mark;
// Find closest point in mesh face
FindIntersection(ray, closest, min_t, max_t,
IsCompatible, compatible_data, face_container->face);
}
}
}
void World::DoCollisionDetection()
{
contacts_.clear();
// °ø - ¹Ù´Ú Ãæµ¹ °Ë»ç
// °ø Z ÁÂÇ¥ - Radius¿Í ¹Ù´Ú°úÀÇ °Å¸® °è»ê
ball_.GetRigidBody().Moved() = false;
float distanceFromFloor = (ball_.GetRigidBody().GetPosition().Z() - ball_.CoveringSphere().Radius()) - floor_.GetPosition().Z();
if(distanceFromFloor < Math::EPSILON)
{
Contact contact;
contact.A = &floor_;
contact.B = &ball_.GetRigidBody();
contact.isVFContact = true;
contact.N = Vector3::UNIT_Z;
contact.PA = Vector3::ZERO;
contact.PB = Vector3(ball_.CoveringSphere().Center() - Vector3(0.0f, 0.0f, ball_.CoveringSphere().Radius()));
ball_.GetRigidBody().SetPosition(ball_.GetRigidBody().GetPosition() - distanceFromFloor*contact.N);
ball_.GetRigidBody().Moved() = true;
contacts_.push_back(contact);
}
// ÇÉ ¹Ù´Ú Ãæµ¹ °Ë»ç
// ÇÉÀ» Box·Î º¸°í... BoxÀÇ °¢ Á¡(8°³)µéÀÌ ¹Ù´Ú°ú À§ÀÎÁö °Ë»ç
for(uint i = 0; i < pins_.size(); ++i)
{
pins_[i].GetRigidBody().Moved() = false;
Vector3 box[BoxVerticeCount];
pins_[i].GetBox(box);
int hitDeepestIndex = -1;
float distanceFromFloorMax = 0.0f;
for(uint j = 0; j < BoxVerticeCount; ++j)
{
float distanceFromFloor = box[j].Z() - floor_.GetPosition().Z();
// Vertex°¡ Áö¸é°ú Á¢ÃËÇϰųª ¾Æ·¡¿¡ ÀÖ´Ù¸é
if(distanceFromFloor < Math::EPSILON)
{
Contact contact;
contact.A = &floor_;
contact.B = &pins_[i].GetRigidBody();
contact.isVFContact = true;
contact.N = Vector3::UNIT_Z; // Áö¸é ¹ý¼±
contact.PA = Vector3::ZERO;
contact.PB = Vector3(box[j]);
pins_[i].GetRigidBody().Moved() = true;
contacts_.push_back(contact);
}
// ¿©·¯ VertexµéÀÌ Áö¸é ¾Æ·¡¿¡ ÀÖÀ» °æ¿ì °¡Àå ¾Æ·¡ÀÎ Vertex¸¦ ±¸ÇصÒ(¾Æ·¡¿¡¼ ±× ¸¸Å Áö¸éÀ¸·Î ²ø¾î¿Ã¸²)
if(distanceFromFloor < distanceFromFloorMax)
{
hitDeepestIndex = j;
distanceFromFloorMax = distanceFromFloor;
}
}
if(hitDeepestIndex != -1)
{
pins_[i].GetRigidBody().SetPosition(pins_[i].GetRigidBody().GetPosition() - distanceFromFloorMax*Vector3::UNIT_Z);
}
}
//
// °ø°ú ÇÉ Ãæµ¹ °Ë»ç
//
for(uint i = 0; i < pins_.size(); ++i)
{
Box box;
pins_[i].GetBox(box);
Sphere& sphere = ball_.CoveringSphere();
Vector3 boxVelocity = pins_[i].GetRigidBody().GetLinearVelocity();
Vector3 sphereVelocity = ball_.GetRigidBody().GetLinearVelocity();
float outTFirst;
float inTMax = 0.1f;
int outQuantity;
Vector3 outP;
if(FindIntersection(box, boxVelocity, sphere, sphereVelocity, outTFirst, inTMax, outQuantity, outP))
{
Contact contact;
contact.isVFContact = true;
contact.A = &pins_[i].GetRigidBody();
contact.B = &ball_.GetRigidBody();
contact.PA = outP + Vector3(0.2f, 0.0f, 0.0f);
contact.PB = outP;
contact.N = Vector3(0.0f, 1.0f, 0.0f); // * Todo: À§¿¡¼ ±¸ÇÑ Á¢Á¡°ú BoxÀÇ 8¸é »çÀÌ °Å¸®°¡ °¡Àå ¤FÀº ¸éÀÇ ¹ý¼± º¤Å͸¦ ±¸ÇÔ
contacts_.push_back(contact);
}
}
//
// ÇÉ°ú ÇÉ Ãæµ¹ °Ë»ç
//
for(uint i = 0; i < pins_.size() - 1; ++i)
{
for(uint j = i + 1; j < pins_.size(); ++j)
{
}
}
//// test for tetrahedron-tetrahedron collisions
//m_iLCPCount = 0;
//for (i = 0; i < NUM_TETRA-1; i++)
//{
// Vector3f akVertex0[4];
// m_apkTetra[i]->GetVertices(akVertex0);
// for (j = i+1; j < NUM_TETRA; j++)
// {
// Vector3f akVertex1[4];
// m_apkTetra[j]->GetVertices(akVertex1);
// if ( !FarApart(i,j) )
// {
// float fDist = 1.0f;
// int iStatusCode = 0;
// Vector3f akRes[2];
// LCPPolyDist3(4,akVertex0,4,m_akFaces,4,akVertex1,4,m_akFaces,
// iStatusCode,fDist,akRes);
// m_iLCPCount++;
// if ( fDist <= m_fTolerance )
// {
// // collision with good LCPPolyDist results
// Reposition(i,j,kContact);
// m_kContact.push_back(kContact);
// }
// }
// }
//}
}
int dCollideBR(dxGeom* RayGeom, dxGeom* BoxGeom, int Flags, dContactGeom* Contacts, int Stride){
const dVector3& Position = *(const dVector3*)dGeomGetPosition(BoxGeom);
const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(BoxGeom);
dVector3 Extents;
dGeomBoxGetLengths(BoxGeom, Extents);
Extents[0] /= 2;
Extents[1] /= 2;
Extents[2] /= 2;
Extents[3] /= 2;
dVector3 Origin, Direction;
dGeomRayGet(RayGeom, Origin, Direction);
dReal Length = dGeomRayGetLength(RayGeom);
dVector3 Diff;
Diff[0] = Origin[0] - Position[0];
Diff[1] = Origin[1] - Position[1];
Diff[2] = Origin[2] - Position[2];
Diff[3] = Origin[3] - Position[3];
Direction[0] *= Length;
Direction[1] *= Length;
Direction[2] *= Length;
Direction[3] *= Length;
dVector3 Rot[3];
Decompose(Rotation, Rot);
dVector3 TransOrigin;
TransOrigin[0] = dDOT(Diff, Rot[0]);
TransOrigin[1] = dDOT(Diff, Rot[1]);
TransOrigin[2] = dDOT(Diff, Rot[2]);
TransOrigin[3] = REAL(0.0);
dVector3 TransDirection;
TransDirection[0] = dDOT(Direction, Rot[0]);
TransDirection[1] = dDOT(Direction, Rot[1]);
TransDirection[2] = dDOT(Direction, Rot[2]);
TransDirection[3] = REAL(0.0);
dReal T[2];
T[0] = 0.0f;
T[1] = dInfinity;
bool Intersect = FindIntersection(TransOrigin, TransDirection, Extents, T[0], T[1]);
if (Intersect){
if (T[0] > REAL(0.0)){
dContactGeom* Contact0 = CONTACT(Flags, Contacts, 0, Stride);
Contact0->pos[0] = Origin[0] + T[0] * Direction[0];
Contact0->pos[1] = Origin[1] + T[0] * Direction[1];
Contact0->pos[2] = Origin[2] + T[0] * Direction[2];
Contact0->pos[3] = Origin[3] + T[0] * Direction[3];
//Contact0->normal = 0;
Contact0->depth = 0.0f;
Contact0->g1 = RayGeom;
Contact0->g2 = BoxGeom;
dContactGeom* Contact1 = CONTACT(Flags, Contacts, 1, Stride);
Contact1->pos[0] = Origin[0] + T[1] * Direction[0];
Contact1->pos[1] = Origin[1] + T[1] * Direction[1];
Contact1->pos[2] = Origin[2] + T[1] * Direction[2];
Contact1->pos[3] = Origin[3] + T[1] * Direction[3];
//Contact1->normal = 0;
Contact1->depth = 0.0f;
Contact1->g1 = RayGeom;
Contact1->g2 = BoxGeom;
return 2;
}
else{
dContactGeom* Contact = CONTACT(Flags, Contacts, 0, Stride);
Contact->pos[0] = Origin[0] + T[1] * Direction[0];
Contact->pos[1] = Origin[1] + T[1] * Direction[1];
Contact->pos[2] = Origin[2] + T[1] * Direction[2];
Contact->pos[3] = Origin[3] + T[1] * Direction[3];
//Contact->normal = 0;
Contact->depth = 0.0f;
Contact->g1 = RayGeom;
Contact->g2 = BoxGeom;
return 1;
}
}
else return 0;
}
