开通博客赚积分
发布资源赚积分
分类

源码开发语言/平台
当前位置：首页 > 源码/资料 > 游戏 > 游戏引擎 > 查看源码
collide.cpp
资源名称：Engine_v0.2.tar.gz　[点击查看]
上传用户：qccn516
上传日期：2013-05-02
资源大小：3382k
文件大小：22k
源码类别：

游戏引擎

开发平台：

Visual C++

collide.cpp：源码内容
  1. /* Collide
  2.  *
  3.  * Copyright (C) 2003-2004, Alexander Zaprjagaev <frustum@frustum.org>
  4.  *
  5.  * This program is free software; you can redistribute it and/or modify
  6.  * it under the terms of the GNU General Public License as published by
  7.  * the Free Software Foundation; either version 2 of the License, or
  8.  * (at your option) any later version.
  9.  *
  10.  * This program is distributed in the hope that it will be useful,
  11.  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  12.  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13.  * GNU General Public License for more details.
  14.  *
  15.  * You should have received a copy of the GNU General Public License
  16.  * along with this program; if not, write to the Free Software
  17.  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  18.  */
  20. #include "engine.h"
  21. #include "object.h"
  22. #include "objectmesh.h"
  23. #include "objectskinnedmesh.h"
  24. #include "mesh.h"
  25. #include "skinnedmesh.h"
  26. #include "rigidbody.h"
  27. #include "collide.h"
  29. int Collide::counter;
  30. Position Collide::position;
  31. int Collide::num_surfaces;
  32. Collide::Surface *Collide::surfaces;
  34. /*
  35.  */
  36. Collide::Collide() : num_contacts(0), num_objects(0) {
  38. contacts = new Contact[NUM_CONTACTS];
  39. objects = new Object*[NUM_OBJECTS];
  41. if(counter++ == 0) {
  42. surfaces = new Surface[NUM_SURFACES];
  43. for(int i = 0; i < NUM_SURFACES; i++) {
  44. surfaces[i].triangles = new Triangle[NUM_TRIANGLES];
  45. }
  46. }
  47. }
  49. Collide::~Collide() {
  51. delete contacts;
  52. delete objects;
  54. if(--counter == 0) {
  55. for(int i = 0; i < NUM_SURFACES; i++) {
  56. delete surfaces[i].triangles;
  57. }
  58. delete surfaces;
  59. }
  60. }
  62. /*****************************************************************************/
  63. /*                                                                           */
  64. /* add contact                                                               */
  65. /*                                                                           */
  66. /*****************************************************************************/
  68. int Collide::addContact(Object *object,Material *material,const vec3 &point,const vec3 &normal,float depth,int min_depth) {
  69. Contact *c = &contacts[num_contacts++];
  70. if(min_depth) {
  71. vec3 p = object->is_identity ? point : object->transform * point;
  72. for(int i = 0; i < num_contacts; i++) {
  73. if(contacts[i].point == p) {
  74. num_contacts--;
  75. if(contacts[i].depth < depth) return 1;
  76. c = &contacts[i];
  77. break;
  78. }
  79. }
  80. }
  81. if(num_contacts == NUM_CONTACTS) {
  82. num_contacts--;
  83. return 0;
  84. }
  85. if(num_objects == 0) {
  86. objects[num_objects++] = object;
  87. } else {
  88. for(int i = 0; i < num_objects; i++) {
  89. if(objects[i] == object) break;
  90. else if(i == num_objects - 1) objects[num_objects++] = object;
  91. }
  92. }
  93. c->object = object;
  94. c->material = material;
  95. c->point = object->is_identity ? point : object->transform * point;
  96. c->normal = object->is_identity ? normal : object->transform.rotation() * normal;
  97. c->depth = depth;
  99. /*glDepthFunc(GL_ALWAYS);
  100. glPointSize(5.0);
  101. glColor3f(0,1,0);
  102. glBegin(GL_POINTS);
  103. glVertex3fv(c->point);
  104. glEnd();
  105. glColor3f(0,0,1);
  106. glBegin(GL_LINES);
  107. glVertex3fv(c->point);
  108. glVertex3fv(c->point + c->normal * c->depth * 20);
  109. glEnd();*/
  111. return 1;
  112. }
  114. /*****************************************************************************/
  115. /*                                                                           */
  116. /* collide with sphere                                                       */
  117. /*                                                                           */
  118. /*****************************************************************************/
  120. int Collide::collide(Object *object,const vec3 &pos,float radius) {
  121. position = pos;
  122. return collide(object,position,radius);
  123. }
  125. int Collide::collide(Object *object,const Position &pos,float radius) {
  126. num_contacts = 0;
  127. num_objects = 0;
  129. if(Bsp::num_sectors == 0) return 0;
  130. if(pos.sector == -1) return 0;
  132. for(int i = 0; i < pos.num_sectors; i++) {
  133. Sector *s = &Bsp::sectors[pos.sectors[i]];
  134. for(int j = 0; j < s->num_node_objects; j++) { // static objects
  135. Object *o = s->node_objects[j];
  136. if((o->getCenter() - pos).length() >= o->getRadius() + radius) continue;
  137. collideObjectSphere(o,pos,radius);
  138. }
  139. for(int j = 0; j < s->num_objects; j++) { // dynamic objects
  140. Object *o = s->objects[j];
  141. if(object == o) continue;
  142. vec3 p = o->is_identity ? pos : o->itransform * pos;
  143. if((o->getCenter() - p).length() >= o->getRadius() + radius) continue;
  144. if(object && object->rigidbody && object->rigidbody->num_joints > 0 && o->rigidbody) {
  145. RigidBody *rb = object->rigidbody;
  146. int k;
  147. for(k = 0; k < rb->num_joints; k++) {
  148. if(rb->joined_rigidbodies[k] == o->rigidbody) break;
  149. }
  150. if(k == rb->num_joints) collideObjectSphere(o,p,radius);
  151. } else {
  152. collideObjectSphere(o,p,radius);
  153. }
  154. }
  155. }
  156. return num_contacts;
  157. }
  159. /*
  160.  */
  161. void Collide::collideObjectSphere(Object *object,const vec3 &pos,float radius) {
  163. static int next[3] = { 1, 2, 0 };
  165. // collide sphere-Mesh
  166. if(object->type == Object::OBJECT_MESH) {
  168. // collide sphere-sphere
  169. if(object->rigidbody && object->rigidbody->collide_type == RigidBody::BODY_SPHERE) {
  170. float r = object->getRadius();
  171. if(pos.length() < radius + r) {
  172. vec3 normal = pos;
  173. normal.normalize();
  174. if(!addContact(object,NULL,normal * radius,normal,radius + r - pos.length())) return;
  175. }
  176. return;
  177. }
  179. // collide sphere-Mesh
  180. Mesh *mesh = reinterpret_cast<ObjectMesh*>(object)->mesh;
  182. for(int i = 0; i < mesh->getNumSurfaces(); i++) {
  183. if((mesh->getCenter(i) - pos).length() > mesh->getRadius(i) + radius) continue;
  185. Mesh::Triangle *triangles = mesh->getTriangles(i);
  187. int num_triangles = mesh->getNumTriangles(i);
  188. for(int j = 0; j < num_triangles; j++) {
  189. Mesh::Triangle *t = &triangles[j];
  191. // distance from the center of sphere to the plane of triangle
  192. float dist = vec4(pos,1) * t->plane;
  193. if(dist >= radius || dist < 0) continue;
  195. vec3 normal = t->plane;
  196. vec3 point = pos - normal * radius;
  197. float depth = radius - dist;
  199. int k; // point in traingle
  200. for(k = 0; k < 3; k++) if(vec4(point,1) * t->c[k] < 0.0) break;
  202. // collide sphere with edges
  203. if(k != 3) {
  204. point = pos - normal * dist; // point on triangle plane
  205. for(k = 0; k < 3; k++) {
  206. vec3 edge = t->v[next[k]] - t->v[k];
  207. vec3 dir = cross(edge,normal);
  208. dir.normalize();
  210. float d = (point - t->v[k]) * dir;
  211. if(d >= radius || d <= 0) continue;
  213. vec3 p = point - dir * d; // point on edge
  215. float dot = p * edge; // clamp point
  216. if(dot > t->v[next[k]] * edge) p = t->v[next[k]];
  217. else if(dot < t->v[k] * edge) p = t->v[k];
  219. d = (point - p).length();
  220. if(d > radius) continue;
  222. depth = sqrt(radius * radius - d * d) - dist;
  223. if(depth <= 0.0) continue;
  225. point = p - normal * depth;
  227. break; // ok
  228. }
  229. if(k == 3) continue; // next triangle
  230. }
  232. // add new contact
  233. if(!addContact(object,object->materials[i],point,t->plane,depth)) return;
  234. }
  235. }
  236. return;
  237. }
  239. // collide sphere-SkinnedMesh
  240. /*if(object->type == Object::OBJECT_SKINNEDMESH) {
  242. ObjectSkinnedMesh *objectskinnedmesh = reinterpret_cast<ObjectSkinnedMesh*>(object);
  243. if(objectskinnedmesh->ragdoll) return;
  245. SkinnedMesh *skinnedmesh = objectskinnedmesh->skinnedmesh;
  247. for(int i = 0; i < skinnedmesh->getNumSurfaces(); i++) {
  248. if((skinnedmesh->getCenter(i) - pos).length() > skinnedmesh->getRadius(i) + radius) continue;
  250. SkinnedMesh::Vertex *vertex = skinnedmesh->getVertex(i);
  251. SkinnedMesh::Triangle *triangles = skinnedmesh->getTriangles(i);
  253. int num_triangles = skinnedmesh->getNumTriangles(i);
  254. for(int j = 0; j < num_triangles; j++) {
  255. SkinnedMesh::Triangle *t = &triangles[j];
  257. // distance from the center of sphere to the plane of triangle
  258. float dist = vec4(pos,1) * t->plane;
  259. if(dist >= radius || dist < 0) continue;
  261. vec3 normal = vec3(t->plane);
  262. vec3 point = pos - normal * radius;
  263. float depth = radius - dist;
  265. int k; // point in traingle
  266. for(k = 0; k < 3; k++) if(vec4(point,1) * t->c[k] < 0.0) break;
  268. // collide sphere with edges
  269. if(k != 3) {
  270. point = pos - normal * dist; // point on triangle plane
  271. for(k = 0; k < 3; k++) {
  272. float d = (point - vertex[t->v[k]].xyz) * t->c[k];
  273. if(d >= radius || d <= 0) continue;
  275. vec3 edge = vertex[t->v[k]].xyz - vertex[t->v[next[k]]].xyz;
  276. vec3 p = point - t->c[k] * d; // point on edge
  278. float dot = p * edge; // clamp point
  279. if(dot > vertex[t->v[next[k]]].xyz * edge) p = vertex[t->v[next[k]]].xyz;
  280. else if(dot < vertex[t->v[k]].xyz * edge) p = vertex[t->v[k]].xyz;
  282. d = (point - p).length();
  283. if(d > radius) continue;
  285. depth = sqrt(radius * radius - d * d) - dist;
  286. if(depth <= 0.0) continue;
  288. point = p - normal * depth;
  290. break; // ok
  291. }
  292. if(k == 3) continue; // next triangle
  293. }
  295. // add new contact
  296. if(!addContact(object,object->materials[i],point,normal,depth)) return;
  297. }
  298. }
  299. return;
  300. }*/
  301. }
  303. /*****************************************************************************/
  304. /*                                                                           */
  305. /* collide with mesh                                                         */
  306. /*                                                                           */
  307. /*****************************************************************************/
  309. int Collide::collide(Object *object) {
  311. num_contacts = 0;
  312. num_objects = 0;
  314. if(Bsp::num_sectors == 0) return 0;
  315. if(object->pos.sector == -1) return 0;
  317. num_surfaces = 0;
  319. // transform and copy triangles from the mesh in to collide surface
  320. if(object->type == Object::OBJECT_MESH) {
  322. Mesh *mesh = reinterpret_cast<ObjectMesh*>(object)->mesh;
  324. for(int i = 0; i < mesh->getNumSurfaces(); i++) {
  326. surfaces[num_surfaces].num_triangles = mesh->getNumTriangles(i);
  328. if(surfaces[num_surfaces].num_triangles > NUM_TRIANGLES) {
  329. fprintf(stderr,"Collide::collide(): many vertex %dn",surfaces[num_surfaces].num_triangles);
  330. continue;
  331. }
  333. Mesh::Triangle *t = mesh->getTriangles(i);
  334. Triangle *ct = surfaces[num_surfaces].triangles;
  336. vec3 min = vec3(1000000,1000000,1000000); // calculate bound box
  337. vec3 max = vec3(-1000000,-1000000,-1000000);
  339. for(int j = 0; j < surfaces[num_surfaces].num_triangles; j++) {
  340. ct[j].v[0] = object->transform * t[j].v[0];
  341. ct[j].v[1] = object->transform * t[j].v[1];
  342. ct[j].v[2] = object->transform * t[j].v[2];
  343. mat4 rotate = object->transform.rotation();
  344. vec3 normal = rotate * vec3(t[j].plane);
  345. ct[j].plane = vec4(normal,-ct[j].v[0] * normal);
  346. normal = rotate * vec4(t[j].c[0]);
  347. ct[j].c[0] = vec4(normal,-ct[j].v[0] * normal);
  348. normal = rotate * vec4(t[j].c[1]);
  349. ct[j].c[1] = vec4(normal,-ct[j].v[1] * normal);
  350. normal = rotate * vec4(t[j].c[2]);
  351. ct[j].c[2] = vec4(normal,-ct[j].v[2] * normal);
  352. for(int k = 0; k < 3; k++) {
  353. if(min.x > ct[j].v[k].x) min.x = ct[j].v[k].x;
  354. else if(max.x < ct[j].v[k].x) max.x = ct[j].v[k].x;
  355. if(min.y > ct[j].v[k].y) min.y = ct[j].v[k].y;
  356. else if(max.y < ct[j].v[k].y) max.y = ct[j].v[k].y;
  357. if(min.z > ct[j].v[k].z) min.z = ct[j].v[k].z;
  358. else if(max.z < ct[j].v[k].z) max.z = ct[j].v[k].z;
  359. }
  360. }
  361. // bound sphere
  362. surfaces[num_surfaces].center = (max + min) / 2.0f;
  363. surfaces[num_surfaces].radius = (max - min).length() / 2.0f;
  364. // bound box
  365. surfaces[num_surfaces].min = min;
  366. surfaces[num_surfaces].max = max;
  368. // new surface
  369. num_surfaces++;
  370. if(num_surfaces == NUM_SURFACES) {
  371. fprintf(stderr,"Collide::collide(): many surfaces %dn",NUM_SURFACES);
  372. break;
  373. }
  374. }
  375. } else {
  376. fprintf(stderr,"Collide::collide(): %d format isn`t supportedn",object->type);
  377. }
  379. if(!num_surfaces) return 0;
  381. // static objects
  382. for(int i = 0; i < object->pos.num_sectors; i++) {
  383. Sector *s = &Bsp::sectors[object->pos.sectors[i]];
  384. for(int j = 0; j < s->num_node_objects; j++) {
  385. Object *o = s->node_objects[j];
  386. if((o->getCenter() - object->pos - object->getCenter()).length() >= o->getRadius() + object->getRadius()) continue;
  387. collideObjectMesh(o);
  388. }
  389. }
  391. // dynamic objects
  392. for(int i = 0; i < object->pos.num_sectors; i++) {
  394. Sector *s = &Bsp::sectors[object->pos.sectors[i]];
  396. for(int j = 0; j < s->num_objects; j++) {
  398. Object *o = s->objects[j];
  399. if(object == o) continue;
  401. if((o->pos + o->getCenter() - object->pos - object->getCenter()).length() >= o->getRadius() + object->getRadius()) continue;
  402. mat4 transform = o->itransform * object->transform;
  404. if(object->rigidbody && object->rigidbody->num_joints > 0 && o->rigidbody) {
  405. RigidBody *rb = object->rigidbody;
  406. int k;
  407. for(k = 0; k < rb->num_joints; k++) {
  408. if(rb->joined_rigidbodies[k] == o->rigidbody) break;
  409. }
  410. if(k != rb->num_joints) continue;
  411. }
  413. // object bound box
  414. vec3 object_min = vec3(1000000,1000000,1000000);
  415. vec3 object_max = vec3(-1000000,-1000000,-1000000);
  417. // transform vertex and calcualte bound box
  418. num_surfaces = 0;
  419. if(object->type == Object::OBJECT_MESH) {
  420. Mesh *mesh = reinterpret_cast<ObjectMesh*>(object)->mesh;
  421. for(int k = 0; k < mesh->getNumSurfaces(); k++) {
  423. surfaces[num_surfaces].num_triangles = mesh->getNumTriangles(k);
  424. if(surfaces[num_surfaces].num_triangles > NUM_TRIANGLES) continue;
  426. Mesh::Triangle *t = mesh->getTriangles(k);
  427. Triangle *ct = surfaces[num_surfaces].triangles;
  429. vec3 min = vec3(1000000,1000000,1000000); // calculate bound box
  430. vec3 max = vec3(-1000000,-1000000,-1000000);
  431. for(int l = 0; l < surfaces[num_surfaces].num_triangles; l++) {
  432. ct[l].v[0] = transform * t[l].v[0];
  433. ct[l].v[1] = transform * t[l].v[1];
  434. ct[l].v[2] = transform * t[l].v[2];
  435. for(int m = 0; m < 3; m++) {
  436. if(min.x > ct[l].v[m].x) min.x = ct[l].v[m].x;
  437. else if(max.x < ct[l].v[m].x) max.x = ct[l].v[m].x;
  438. if(min.y > ct[l].v[m].y) min.y = ct[l].v[m].y;
  439. else if(max.y < ct[l].v[m].y) max.y = ct[l].v[m].y;
  440. if(min.z > ct[l].v[m].z) min.z = ct[l].v[m].z;
  441. else if(max.z < ct[l].v[m].z) max.z = ct[l].v[m].z;
  442. }
  443. }
  444. // bound sphere
  445. surfaces[num_surfaces].center = (max + min) / 2.0f;
  446. surfaces[num_surfaces].radius = (max - min).length() / 2.0f;
  447. // bound box
  448. surfaces[num_surfaces].min = min;
  449. surfaces[num_surfaces].max = max;
  450. // object bound box
  451. if(object_min.x > min.x) object_min.x = min.x;
  452. if(object_max.x < max.x) object_max.x = max.x;
  453. if(object_min.y > min.y) object_min.y = min.y;
  454. if(object_max.y < max.y) object_max.y = max.y;
  455. if(object_min.z > min.z) object_min.z = min.z;
  456. if(object_max.z < max.z) object_max.z = max.z;
  458. num_surfaces++;
  459. if(num_surfaces == NUM_SURFACES) break;
  460. }
  461. }
  463. if(num_surfaces == 0) continue;
  465. // bound box test
  466. const vec3 &min = o->getMin();
  467. const vec3 &max = o->getMax();
  469. if(min.x >= object_max.x) continue;
  470. if(max.x <= object_min.x) continue;
  471. if(min.y >= object_max.y) continue;
  472. if(max.y <= object_min.y) continue;
  473. if(min.z >= object_max.z) continue;
  474. if(max.z <= object_min.z) continue;
  476. // recalculate normals and fast point and triangle
  477. num_surfaces = 0;
  478. if(object->type == Object::OBJECT_MESH) {
  479. Mesh *mesh = reinterpret_cast<ObjectMesh*>(object)->mesh;
  480. for(int k = 0; k < mesh->getNumSurfaces(); k++) {
  482. surfaces[num_surfaces].num_triangles = mesh->getNumTriangles(k);
  483. if(surfaces[num_surfaces].num_triangles > NUM_TRIANGLES) continue;
  485. Mesh::Triangle *t = mesh->getTriangles(k);
  486. Triangle *ct = surfaces[num_surfaces].triangles;
  488. for(int l = 0; l < surfaces[num_surfaces].num_triangles; l++) {
  489. mat4 rotate = transform.rotation();
  490. vec3 normal = rotate * vec3(t[l].plane);
  491. ct[l].plane = vec4(normal,-ct[l].v[0] * normal);
  492. normal = rotate * vec4(t[l].c[0]);
  493. ct[l].c[0] = vec4(normal,-ct[l].v[0] * normal);
  494. normal = rotate * vec4(t[l].c[1]);
  495. ct[l].c[1] = vec4(normal,-ct[l].v[1] * normal);
  496. normal = rotate * vec4(t[l].c[2]);
  497. ct[l].c[2] = vec4(normal,-ct[l].v[2] * normal);
  498. }
  500. num_surfaces++;
  501. if(num_surfaces == NUM_SURFACES) break;
  502. }
  503. }
  504. if(num_surfaces == 0) continue;
  506. // collide it
  507. collideObjectMesh(o);
  508. }
  509. }
  511. return num_contacts;
  512. }
  514. /*
  515.  */
  516. void Collide::collideObjectMesh(Object *object) {
  518. static int next[3] = { 1, 2, 0 };
  520. // collide Mesh-Mesh
  521. if(object->type == Object::OBJECT_MESH) {
  523. Mesh *mesh = reinterpret_cast<ObjectMesh*>(object)->mesh;
  525. for(int i = 0; i < mesh->getNumSurfaces(); i++) {
  526. for(int j = 0; j < num_surfaces; j++) {
  528. if((mesh->getCenter(i) - surfaces[j].center).length() >= mesh->getRadius(i) + surfaces[j].radius) continue;
  530. const vec3 &min = mesh->getMin(i);
  531. const vec3 &max = mesh->getMax(i);
  533. if(min.x >= surfaces[j].max.x) continue;
  534. if(max.x <= surfaces[j].min.x) continue;
  535. if(min.y >= surfaces[j].max.y) continue;
  536. if(max.y <= surfaces[j].min.y) continue;
  537. if(min.z >= surfaces[j].max.z) continue;
  538. if(max.z <= surfaces[j].min.z) continue;
  540. Mesh::Triangle *triangles = mesh->getTriangles(i);
  541. Surface *s = &surfaces[j];
  543. int num_triangles = mesh->getNumTriangles(i);
  544. for(int k = 0; k < num_triangles; k++) {
  546. Mesh::Triangle *t = &triangles[k];
  548. for(int l = 0; l < s->num_triangles; l++) {
  550. Triangle *ct = &s->triangles[l];
  552. // fast triangle-triangle intersection test
  553. float dist0[3];
  554. int collide0 = 0;
  555. for(int m = 0; m < 3; m++) {
  556. dist0[m] = ct->v[m] * t->plane;
  557. if(fabs(dist0[m]) > s->radius * 2) {
  558. collide0 = 3;
  559. break;
  560. }
  561. if(dist0[m] > 0.0) collide0++;
  562. else if(dist0[m] < 0.0) collide0--;
  563. }
  564. if(collide0 == 3 || collide0 == -3) continue;
  566. for(int m = 0; m < 3; m++) {
  567. if(!((dist0[m] <= 0.0) ^ (dist0[next[m]] <= 0.0))) continue;
  569. vec3 &v0 = ct->v[m];
  570. vec3 &v1 = ct->v[next[m]];
  571. vec3 edge = v1 - v0;
  572. vec3 p = v0 - edge * dist0[m] / (dist0[next[m]] - dist0[m]); // intersect ct edge with t plane
  574. int p_inside = p * t->c[0] > 0.0 && p * t->c[1] > 0.0 && p * t->c[2] > 0.0;
  575. if(!p_inside) continue;
  577. int v0_inside = v0 * t->c[0] > 0.0 && v0 * t->c[1] > 0.0 && v0 * t->c[2] > 0.0;
  578. int v1_inside = v1 * t->c[0] > 0.0 && v1 * t->c[1] > 0.0 && v1 * t->c[2] > 0.0;
  580. if(v0_inside && v1_inside) {
  581. float length = edge.length() / 2.0f;
  582. if(dist0[m] < 0.0 && dist0[m] > -length) {
  583. if(!addContact(object,object->materials[i],v0,t->plane,-dist0[m],true)) return;
  584. } else if(dist0[next[m]] < 0.0 && dist0[next[m]] > -length) {
  585. if(!addContact(object,object->materials[i],v1,t->plane,-dist0[next[m]],true)) return;
  586. }
  587. }
  589. if(v0_inside != v1_inside) {
  590. float dist = 1000000;
  591. for(int n = 0; n < 3; n++) {
  592. //float d = t->v[n] * t->c[n];
  593. float d = p * t->c[n];
  594. if(dist > d) dist = d;
  595. }
  596. if(dist0[m] < 0.0) {
  597. float d = (v0 - p).length();
  598. if(dist > d) dist = 0;
  599. else dist = -dist0[m] * dist / d;
  600. } else {
  601. float d = (v1 - p).length();
  602. if(dist > d) dist = 0;
  603. else dist = -dist0[next[m]] * dist / d;
  604. }
  605. if(!addContact(object,object->materials[i],p,-ct->plane,dist)) return;
  606. }
  607. }
  609. // collide with scene
  610. if(!object->rigidbody) {
  612. float dist1[3];
  613. int collide1 = 0;
  614. for(int m = 0; m < 3; m++) {
  615. dist1[m] = t->v[m] * ct->plane;
  616. if(dist1[m] > 0.0) collide1++;
  617. else if(dist1[m] < 0.0) collide1--;
  618. }
  619. if(collide1 == 3 || collide1 == -3) continue;
  621. for(int m = 0; m < 3; m++) {
  622. if(dist1[m] >= 0) continue;
  623. if(dist1[next[m]] <= 0) continue;
  625. vec3 &v0 = t->v[m];
  626. vec3 &v1 = t->v[next[m]];
  627. vec3 edge = v1 - v0;
  628. vec3 p = v0 - edge * dist1[m] / (dist1[next[m]] - dist1[m]); // intersect ct edge with t plane
  630. int p_inside = p * ct->c[0] > 0.0 && p * ct->c[1] > 0.0 && p * ct->c[2] > 0.0;
  631. if(!p_inside) continue;
  633. int v0_inside = v0 * ct->c[0] > 0.0 && v0 * ct->c[1] > 0.0 && v0 * ct->c[2] > 0.0;
  634. int v1_inside = v1 * ct->c[0] > 0.0 && v1 * ct->c[1] > 0.0 && v1 * ct->c[2] > 0.0;
  636. if(v0_inside != v1_inside) {
  637. float dist = 1000000;
  638. for(int n = 0; n < 3; n++) {
  639. float d = p * ct->c[n];
  640. if(dist > d) dist = d;
  641. }
  642. float d = (v0 - p).length();
  643. if(dist > d) dist = 0;
  644. else dist = -dist1[m] * dist / d;
  645. if(!addContact(object,object->materials[i],p,t->plane,dist)) return;
  646. }
  647. }
  648. }
  649. }
  650. }
  651. }
  652. }
  653. return;
  654. }
  655. }
  657. /*****************************************************************************/
  658. /*                                                                           */
  659. /* sorting contacts                                                          */
  660. /*                                                                           */
  661. /*****************************************************************************/
  663. static int contact_cmp(const void *a,const void *b) {
  664. Collide::Contact *c0 = (Collide::Contact*)a;
  665. Collide::Contact *c1 = (Collide::Contact*)b;
  666. if(c0->object->pos.z > c1->object->pos.z) return 1;
  667. if(c0->object->pos.z < c1->object->pos.z) return -1;
  668. if(c0->point.z > c1->point.z) return 1;
  669. if(c0->point.z < c1->point.z) return -1;
  670. return 0;
  671. }
  673. void Collide::sort() {
  674. qsort(contacts,num_contacts,sizeof(Contact),contact_cmp);
  675. }