69 int pt0 =
models[0]->getPrimitiveType();
70 int pt1 =
models[1]->getPrimitiveType();
72 bool detectPlaneSphereCollisions(
bool detectAllContacts);
115 if (!plane || !mesh || !mesh->dataSet->model.GetMeshInterface())
return false;
121 IceMaths::Matrix4x4 mTrans = *(mesh->transform);
123 for(
udword j=0; j<3; j++){
131 IceMaths::Matrix4x4 pTrans = (*(plane->pTransform)) * (*(plane->transform));
132 IceMaths::Point p, nLocal(0,0,1),
n;
136 bool IsOk = PC.
Collide(Cache, Planes, 1, mesh->dataSet->model,
139 std::cerr <<
"PlanesCollider::Collide() failed" << std::endl;
144 std::vector<collision_data>& cdata
146 for (
size_t i=0;
i<cdata.size();
i++){
147 cdata[
i].n_vector *= -1;
160 Opcode::BVTCache colCache;
164 colCache.Model0 = &
models[1]->dataSet->model;
165 colCache.Model1 = &
models[0]->dataSet->model;
167 if(colCache.Model0->HasSingleNode() || colCache.Model1->HasSingleNode())
170 Opcode::AABBTreeCollider collider;
173 if(!detectAllContacts){
174 collider.SetFirstContact(
true);
180 std::cerr <<
"AABBTreeCollider::Collide() failed" << std::endl;
182 result = collider.GetContactStatus();
202 IceMaths::Matrix4x4 sATrans = (*(sphereA->pTransform)) * (*(sphereA->transform));
203 IceMaths::Matrix4x4 sBTrans = (*(sphereB->pTransform)) * (*(sphereB->transform));
205 float radiusA, radiusB;
206 sphereA->getPrimitiveParam(0, radiusA);
207 sphereB->getPrimitiveParam(0, radiusB);
209 IceMaths::Point centerA = sATrans.GetTrans();
210 IceMaths::Point centerB = sBTrans.GetTrans();
212 IceMaths::Point D = centerB - centerA;
214 float depth = radiusA + radiusB - D.Magnitude();
216 if (D.Magnitude() <= (radiusA + radiusB)) {
220 float x = (pow(D.Magnitude(), 2) + pow(radiusA, 2) - pow(radiusB, 2)) / (2 * D.Magnitude());
222 IceMaths::Point
n = D / D.Magnitude();
224 IceMaths::Point q = centerA +
n * x;
242 cdata.push_back(col);
268 if (!sphere || !mesh)
271 IceMaths::Matrix4x4 sTrans = (*(sphere->pTransform)) * (*(sphere->transform));
274 sphere->getPrimitiveParam(0, radius);
276 IceMaths::Sphere sphere_def(IceMaths::Point(0, 0, 0), radius);
278 Opcode::SphereCache colCache;
280 Opcode::SphereCollider collider;
282 if (!detectAllContacts) {
283 collider.SetFirstContact(
true);
286 bool isOk = collider.Collide(colCache, sphere_def, mesh->dataSet->model, &sTrans, mesh->transform);
290 if (collider.GetContactStatus()) {
292 int TouchedPrimCount = collider.GetNbTouchedPrimitives();
293 const udword* TouchedPrim = collider.GetTouchedPrimitives();
295 if (TouchedPrimCount) {
299 std::vector< std::vector<IceMaths::Point> > triangle(TouchedPrimCount);
300 std::vector<IceMaths::Plane> face(TouchedPrimCount);
302 std::vector<float> depth(TouchedPrimCount);
304 std::vector<IceMaths::Point> q(TouchedPrimCount);
305 std::vector<float> A(TouchedPrimCount);
307 IceMaths::Matrix4x4 sTransInv;
313 for (
int i = 0;
i < TouchedPrimCount;
i++) {
316 std::vector<IceMaths::Point>
vertex(3);
321 mesh->getTriangle(TouchedPrim[
i], vertex_index[0], vertex_index[1], vertex_index[2]);
323 for (
int j = 0; j < 3; j++) {
324 mesh->getVertex(vertex_index[j], x, y, z);
328 triangle[
i] = std::vector<IceMaths::Point> (
vertex);
333 IceMaths::Plane face_s;
337 if (abs(face_s.d) > radius)
338 cout <<
"No intersection";
341 R = sqrt(pow(radius, 2) - pow(face_s.d, 2));
342 depth[
i] = radius - abs(face_s.d);
344 IceMaths::Point U, V;
351 IceMaths::Matrix4x4 scTrans;
352 scTrans.SetRow(0, U);
353 scTrans.SetRow(1, V);
354 scTrans.SetRow(2, face_s.n);
355 scTrans.SetRow(3, face_s.n * -face_s.d);
357 IceMaths::Matrix4x4 scTransInv;
360 IceMaths::Point vertex_c[3];
361 std::vector<float> vx, vy;
363 for (
int j = 0; j < 3; j++) {
365 vx.push_back(vertex_c[j].x);
366 vy.push_back(vertex_c[j].y);
376 std::vector<bool> considered_checklist(TouchedPrimCount,
false);
377 std::vector<int> sameplane;
379 std::vector<IceMaths::Point> new_q;
380 std::vector<IceMaths::Point> new_n;
381 std::vector<float> new_depth;
385 for (
int i = 0;
i < TouchedPrimCount;
i++) {
387 if (!considered_checklist[
i]) {
389 for (
int j =
i + 1; j < TouchedPrimCount; j++) {
390 IceMaths::Point normdiff(face[
i].
n - face[j].
n);
392 if (!sameplane.size()) sameplane.push_back(
i);
393 sameplane.push_back(j);
397 if (!sameplane.size()) {
398 new_q.push_back(q[
i]);
399 new_n.push_back(face[
i].
n);
400 new_depth.push_back(depth[
i]);
401 considered_checklist[
i] =
true;
405 float sum_xA, sum_yA, sum_zA, sum_A;
406 sum_xA = sum_yA = sum_zA = sum_A = 0;
408 for (
unsigned int k = 0; k < sameplane.size(); k++) {
409 sum_xA += q[sameplane[k]].x * A[sameplane[k]];
410 sum_yA += q[sameplane[k]].y * A[sameplane[k]];
411 sum_zA += q[sameplane[k]].z * A[sameplane[k]];
412 sum_A += A[sameplane[k]];
413 considered_checklist[sameplane[k]] =
true;
416 IceMaths::Point q_temp;
417 q_temp.x = sum_xA / sum_A;
418 q_temp.y = sum_yA / sum_A;
419 q_temp.z = sum_zA / sum_A;
420 new_q.push_back(q_temp);
421 new_n.push_back(face[
i].
n);
422 new_depth.push_back(depth[
i]);
429 for (
unsigned int i = 0;
i < new_q.size();
i++) {
443 cdata.push_back(col);
450 std::cerr <<
"SphereCollider::Collide() failed" << std::endl;
472 if (!plane || !cylinder)
return false;
474 IceMaths::Matrix4x4 pTrans = (*(plane->pTransform)) * (*(plane->transform));
475 IceMaths::Matrix4x4 cTrans = (*(cylinder->pTransform)) * (*(cylinder->transform));
478 cylinder->getPrimitiveParam(0, radius);
479 cylinder->getPrimitiveParam(1,
height);
481 IceMaths::Point pTopLocal(0,
height/2, 0), pBottomLocal(0, -
height/2, 0);
482 IceMaths::Point pTop, pBottom;
486 IceMaths::Point pOnPlane, nLocal(0,0,1),
n;
488 pTrans.GetTrans(pOnPlane);
489 float d = pOnPlane|
n;
491 float dTop = (pTop|
n) - d;
492 float dBottom = (pBottom|
n) - d;
494 if (dTop > radius && dBottom > radius)
return false;
496 double theta = asin((dTop - dBottom)/
height);
497 double rcosth = radius*cos(theta);
499 int contactsCount = 0;
500 if (rcosth >= dTop) contactsCount+=2;
501 if (rcosth >= dBottom) contactsCount+=2;
505 cdata.resize(contactsCount);
506 for (
int i=0;
i<contactsCount;
i++){
507 cdata[
i].num_of_i_points = 1;
508 cdata[
i].i_point_new[0]=1;
509 cdata[
i].i_point_new[1]=0;
510 cdata[
i].i_point_new[2]=0;
511 cdata[
i].i_point_new[3]=0;
513 cdata[
i].n_vector[0] = -
n.x;
514 cdata[
i].n_vector[1] = -
n.y;
515 cdata[
i].n_vector[2] = -
n.z;
517 cdata[
i].n_vector[0] =
n.x;
518 cdata[
i].n_vector[1] =
n.y;
519 cdata[
i].n_vector[2] =
n.z;
522 IceMaths::Point vBottomTop = pTop - pBottom;
523 IceMaths::Point v = vBottomTop^
n;
525 IceMaths::Point w = v^
n;
528 unsigned int index=0;
529 if (rcosth >= dBottom){
530 double depth = rcosth - dBottom;
531 IceMaths::Point iPoint = pBottom - dBottom*
n - dBottom*tan(theta)*w;
532 double x = dBottom/cos(theta);
533 IceMaths::Point dv = sqrt(radius*radius - x*x)*v;
534 cdata[index].i_points[0][0] = iPoint.x + dv.x;
535 cdata[index].i_points[0][1] = iPoint.y + dv.y;
536 cdata[index].i_points[0][2] = iPoint.z + dv.z;
537 cdata[index].depth = depth;
539 cdata[index].i_points[0][0] = iPoint.x - dv.x;
540 cdata[index].i_points[0][1] = iPoint.y - dv.y;
541 cdata[index].i_points[0][2] = iPoint.z - dv.z;
542 cdata[index].depth = depth;
546 double depth = rcosth - dTop;
547 IceMaths::Point iPoint = pTop - dTop*
n - dTop*tan(theta)*w;
548 double x = dTop/cos(theta);
549 IceMaths::Point dv = sqrt(radius*radius - x*x)*v;
550 cdata[index].i_points[0][0] = iPoint.x + dv.x;
551 cdata[index].i_points[0][1] = iPoint.y + dv.y;
552 cdata[index].i_points[0][2] = iPoint.z + dv.z;
553 cdata[index].depth = depth;
555 cdata[index].i_points[0][0] = iPoint.x - dv.x;
556 cdata[index].i_points[0][1] = iPoint.y - dv.y;
557 cdata[index].i_points[0][2] = iPoint.z - dv.z;
558 cdata[index].depth = depth;
572 Opcode::BVTCache colCache;
574 colCache.Model0 = &
models[1]->dataSet->model;
575 colCache.Model1 = &
models[0]->dataSet->model;
581 collider.
Distance(colCache, d, p0, p1,
599 Opcode::BVTCache colCache;
601 colCache.Model0 = &
models[1]->dataSet->model;
602 colCache.Model1 = &
models[0]->dataSet->model;
608 collider.
Distance(colCache, d, p0, p1,
616 triangle1 = colCache.id0;
617 triangle0 = colCache.id1;
629 Opcode::BVTCache colCache;
631 colCache.Model0 = &
models[1]->dataSet->model;
632 colCache.Model1 = &
models[0]->dataSet->model;
662 std::vector<pointStruct> point;
664 unsigned int numInter;
665 std::vector<float> x_int(2), y_int(2);
667 for (
i = 0;
i < vx.size();
i++) {
684 for (k = 0; k < numInter; k++) {
687 p.
angle = atan2(y_int[k], x_int[k]);
698 bool finished =
false;
700 std::vector<figStruct> figure;
705 for (
int cont = 1; cont <= 4; cont++)
706 j[cont] = (j[0] + cont) % point.size();
708 if (point[j[0]].
code) {
712 if (point[j[1]].
code) {
721 else if (point[j[2]].
code || point[j[3]].
code || point[j[4]].
code) {
726 if (point[j[2]].
code)
f.p2 = j[2];
727 else if (point[j[3]].
code)
f.p2 = j[3];
728 else if (point[j[4]].
code)
f.p2 = j[4];
735 cout <<
"Error: No intersection detected" << endl;
744 if (((j[0] == 0) && (
start == -1)) || (j[0] ==
start))
750 std::vector<float> x(3, 0);
751 std::vector<float> y(3, 0);
755 for (k = 0; k < figure.size(); k++) {
757 x[1] = point[figure[k].p1].x;
758 y[1] = point[figure[k].p1].y;
759 x[2] = point[figure[k].p2].x;
760 y[2] = point[figure[k].p2].y;
763 for (
int cont = 0; cont < 3; cont++) {
767 figure[k].cx = sumx / 3;
768 figure[k].cy = sumy / 3;
771 th = point[figure[k].p2].angle - point[figure[k].p1].angle;
772 if (th < 0) th +=
TWOPI;
773 figure[k].area = pow(radius, 2) * th / 2;
774 calculateSectorCentroid(figure[k].cx, figure[k].cy, radius, point[figure[k].p1].angle, point[figure[k].p2].angle);
778 float sum_xA, sum_yA, sum_A;
779 sum_xA = sum_yA = sum_A = 0;
781 for (k = 0; k < figure.size(); k++) {
782 sum_xA += figure[k].cx * figure[k].area;
783 sum_yA += figure[k].cy * figure[k].area;
784 sum_A += figure[k].area;
787 if ((figure.size() == 1) && (sum_A == 0)) {
788 cx = point[figure[0].p1].x;
789 cy = point[figure[0].p1].y;
804 A =
TWOPI * pow(radius, 2);
809 A =
TWOPI * pow(radius, 2);
821 if ((vx.size() == 3) && (vy.size() == 3)) {
829 cout <<
"The number of vertices does not correspond to a triangle" << endl;
838 if (vx.size() == vy.size()) {
839 for (
unsigned int i = 0;
i < vx.size();
i++) {
840 area += vx[
i] * vy[(
i + 1) % vx.size()] - vy[
i] * vx[(
i + 1) % vx.size()];
845 cout <<
"The number of coordinates does not match" << endl;
850 #if defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__)
851 #define trunc(x) ((int)(x))
855 float th, psi, phi, g;
858 if (th2 < th1) th +=
TWOPI;
860 g = (abs(th) >
LOCAL_EPSILON) ? 4.0 / 3.0 * radius / th * sin(th / 2) : 2.0 / 3.0 * radius;
863 if (th2 < th1) psi +=
TWOPI;
865 phi = psi / 2 - trunc(psi / 2 /
TWOPI) *
TWOPI;
873 IceMaths::Point v1, v2;
877 for (
int i = 0;
i < 3;
i++) {
879 v1 = IceMaths::Point(vx[
i], vy[
i], 0) - IceMaths::Point(x, y, 0);
880 v2 = IceMaths::Point(vx[(
i + 1) % vx.size()], vy[(
i + 1) % vy.size()], 0) - IceMaths::Point(x, y, 0);
890 anglesum += acos((v1 | v2) / (m1 * m2));
900 float x_test, y_test;
904 float xmin =
min(x1, x2);
905 float xmax =
max(x1, x2);
906 float ymin =
min(y1, y2);
907 float ymax =
max(y1, y2);
909 float v_norm, proy_norm;
910 float x_temp, y_temp;
912 std::vector<float> t;
914 if ((sqrt(pow(x1, 2) + pow(y1, 2)) != radius) && (sqrt(pow(x2, 2) + pow(y2, 2)) != radius)) {
921 m = (y2 - y1) / (x2 - x1);
924 D = 4 * pow(m, 2) * pow(
b, 2) - 4 * (1 + pow(m, 2)) * (pow(
b, 2) - pow(radius, 2));
927 D = pow(radius, 2) - pow(x1, 2);
929 numint = D < 0 ? 0 : (D > 0 ? 2 : 1);
933 for (
int i = 0;
i < numint;
i++) {
936 x_test = (-2 * m *
b + pow(-1.0,
i) * sqrt(D)) / (2 * (1 + pow(m, 2)));
937 y_test = m * x_test +
b;
941 y_test = pow(-1.0,
i) * sqrt(D);
946 if ((xmin <= x_test) && (x_test <= xmax) && (ymin <= y_test) && (y_test <= ymax)) {
949 v_norm = sqrt(pow(x2 - x1, 2) + pow(y2 - y1, 2));
950 proy_norm = sqrt(pow(x_test - x1, 2) + pow(y_test - y1, 2));
951 t.push_back(proy_norm / v_norm);