bool btPrimitiveTriangle::find_triangle_collision_clip_method(btPrimitiveTriangle & other, GIM_TRIANGLE_CONTACT & contacts)
{
btScalar margin = m_margin + other.m_margin;
btVector3 clipped_points[MAX_TRI_CLIPPING];
int clipped_count;
//create planes
// plane v vs U points
GIM_TRIANGLE_CONTACT contacts1;
contacts1.m_separating_normal = m_plane;
clipped_count = clip_triangle(other,clipped_points);
if (clipped_count == 0 )
{
return false;//Reject
}
//find most deep interval face1
contacts1.merge_points(contacts1.m_separating_normal,margin,clipped_points,clipped_count);
if (contacts1.m_point_count == 0) return false; // too far
//Normal pointing to this triangle
contacts1.m_separating_normal *= -1.f;
//Clip tri1 by tri2 edges
GIM_TRIANGLE_CONTACT contacts2;
contacts2.m_separating_normal = other.m_plane;
clipped_count = other.clip_triangle(*this,clipped_points);
if (clipped_count == 0 )
{
return false;//Reject
}
//find most deep interval face1
contacts2.merge_points(contacts2.m_separating_normal,margin,clipped_points,clipped_count);
if (contacts2.m_point_count == 0) return false; // too far
////check most dir for contacts
if (contacts2.m_penetration_depth<contacts1.m_penetration_depth)
{
contacts.copy_from(contacts2);
}
else
{
contacts.copy_from(contacts1);
}
return true;
}
static void
clip_primitives (GL_primitive_list *pl, GLcontext *g, GL_float plane[4])
{
GLrenderstate *r = g->renderstate;
GL_primitive *p = pl->head, *next;
pl->head = NULL;
pl->tail = NULL;
for ( ; p; p = next) {
int bits = 0;
int cull = 1;
int clip = 0;
int i;
next = p->next;
for (i = 0; i < p->nverts; i++) {
int c = (vdot(p->verts[i]->position, plane) < 0.0f);
cull &= c;
clip |= c;
bits = (bits << 1) | c;
}
if (cull)
__glcore_destroy_primitive(p);
else if (!clip)
__glcore_add_primitive(pl, p);
else {
switch (p->nverts) {
case 1: clip_point(pl, g, plane, p, bits); break;
case 2: clip_line(pl, g, plane, p, bits); break;
case 3: clip_triangle(pl, g, plane, p, bits); break;
}
}
}
}
int main(int argc, char* argv[])
{
(void)argc;
(void)argv;
const int width = 640;
const int height = 480;
const int depth = 32;
bool running = true;
SDL_Event event;
std::vector<Vector4f> triangleMesh;
std::vector<Vector4f> workingCopy;
std::vector<Vector4i> finalCopy;
SDL_Init(SDL_INIT_VIDEO);
SDL_Surface* screen = SDL_SetVideoMode(width, height, depth, SDL_DOUBLEBUF | SDL_SWSURFACE);
SDL_WM_SetCaption("MechCore.net Projection Example", NULL);
makeMeshCircle(triangleMesh, 2.0f);
/*
triangleMesh.push_back(Vector4f( 0.0f, 0.5f, 0.0f, 1.0f));
triangleMesh.push_back(Vector4f(-0.5f, -0.5f, 0.0f, 1.0f));
triangleMesh.push_back(Vector4f( 0.5f, -0.5f, 0.0f, 1.0f));
*/
/*
triangleMesh.push_back(Vector4f( 0.5f, 0.5f, 0.0f, 1.0f));
triangleMesh.push_back(Vector4f( -0.5f, 0.5f, 0.0f, 1.0f));
triangleMesh.push_back(Vector4f( 0.5f, -0.5f, 0.0f, 1.0f));
triangleMesh.push_back(Vector4f( 0.5f, -0.5f, 0.0f, 1.0f));
triangleMesh.push_back(Vector4f( -0.5f, 0.5f, 0.0f, 1.0f));
triangleMesh.push_back(Vector4f( -0.5f, -0.5f, 0.0f, 1.0f));
*/
while(running){
while(SDL_PollEvent(&event)){
switch(event.type)
{
case SDL_KEYDOWN:
if(event.key.keysym.sym == SDLK_ESCAPE){
running = false;
}
break;
case SDL_QUIT:
running = false;
break;
}
}
float time = (float)SDL_GetTicks() * 0.001f;
/* We need a new working copy every frame */
workingCopy.resize(triangleMesh.size());
std::copy(triangleMesh.begin(), triangleMesh.end(), workingCopy.begin());
/* world matrix transform */
Matrix4f worldMatrix = translate(Vector4f(0.0f, 0.0f, -3.25f, 1.0f)) * rotateZ(11.175f * time);
/*
rotateY(time * 90.0f) *
rotateX(time * 45.0f) *
rotateZ(time * 22.5f);
*/
/* perspective function is in linealg.h under /include */
Matrix4f clipMatrix = perspective(90.0f, 4.0f/3.0f, 0.01f, 20.0f);
Matrix4f worldClipMatrix = clipMatrix * worldMatrix;
/* Transform our points */
for(unsigned int i=0; i<workingCopy.size(); i+=3){
Vector4f& p1 = workingCopy[i];
Vector4f& p2 = workingCopy[i+1];
Vector4f& p3 = workingCopy[i+2];
p1 = worldClipMatrix * p1;
p2 = worldClipMatrix * p2;
p3 = worldClipMatrix * p3;
}
clip_triangle(workingCopy, Vector4f(-1.0f, 0.0f, 0.0f, 1.0f));
clip_triangle(workingCopy, Vector4f( 1.0f, 0.0f, 0.0f, 1.0f));
clip_triangle(workingCopy, Vector4f( 0.0f, 1.0f, 0.0f, 1.0f));
clip_triangle(workingCopy, Vector4f( 0.0f, -1.0f, 0.0f, 1.0f));
ASSERT(!(workingCopy.size() % 3));
finalCopy.resize(workingCopy.size());
for(unsigned int i=0; i<workingCopy.size(); ++i)
{
/* does not divide w by w */
workingCopy[i] /= workingCopy[i].w;
/* z should be in range of [0, 1] */
workingCopy[i].z = workingCopy[i].z * 0.5f + 0.5f;
/* project to screenspace
project function is in linealg.h under /include */
workingCopy[i] = project(workingCopy[i], (float)width, (float)height);
/* Store as fixedpoint. We want to interpolate 1/w across the edges.
The interpolated 1/w is flipped again, that is w = 1.0 / (v0.w + t*(v1.w - v0.w)) */
finalCopy[i] = Vector4i(
workingCopy[i].x * 65536.0f,
workingCopy[i].y * 65536.0f,
workingCopy[i].z * 65536.0f,
(1.0f / workingCopy[i].w) * 65536.0f
);
}
//SDL_LockSurface(screen);
unsigned int* pixels = static_cast<unsigned int*>(screen->pixels);
/* clear the screen to black */
memset(pixels, 0, sizeof(Uint32) * width * height);
/* draw the triangles */
TriangleSplit(finalCopy);
DrawTriangle(finalCopy, pixels, width, height);
SDL_Flip(screen);
//SDL_UnlockSurface(screen);
}
SDL_Quit();
return 0;
}
//! collides by two sides
SIMD_FORCE_INLINE bool triangle_collision(
const btVector3 & u0,
const btVector3 & u1,
const btVector3 & u2,
GREAL margin_u,
const btVector3 & v0,
const btVector3 & v1,
const btVector3 & v2,
GREAL margin_v,
GIM_TRIANGLE_CONTACT_DATA & contacts)
{
margin = margin_u + margin_v;
tu_vertices[0] = u0;
tu_vertices[1] = u1;
tu_vertices[2] = u2;
tv_vertices[0] = v0;
tv_vertices[1] = v1;
tv_vertices[2] = v2;
//create planes
// plane v vs U points
TRIANGLE_PLANE(tv_vertices[0],tv_vertices[1],tv_vertices[2],tv_plane);
du[0] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[0]);
du[1] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[1]);
du[2] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[2]);
du0du1 = du[0] * du[1];
du0du2 = du[0] * du[2];
if(du0du1>0.0f && du0du2>0.0f) // same sign on all of them + not equal 0 ?
{
if(du[0]<0) //we need test behind the triangle plane
{
distances[0] = GIM_MAX3(du[0],du[1],du[2]);
distances[0] = -distances[0];
if(distances[0]>margin) return false; //never intersect
//reorder triangle v
VEC_SWAP(tv_vertices[0],tv_vertices[1]);
VEC_SCALE_4(tv_plane,-1.0f,tv_plane);
}
else
{
distances[0] = GIM_MIN3(du[0],du[1],du[2]);
if(distances[0]>margin) return false; //never intersect
}
}
else
{
//Look if we need to invert the triangle
distances[0] = (du[0]+du[1]+du[2])/3.0f; //centroid
if(distances[0]<0.0f)
{
//reorder triangle v
VEC_SWAP(tv_vertices[0],tv_vertices[1]);
VEC_SCALE_4(tv_plane,-1.0f,tv_plane);
distances[0] = GIM_MAX3(du[0],du[1],du[2]);
distances[0] = -distances[0];
}
else
{
distances[0] = GIM_MIN3(du[0],du[1],du[2]);
}
}
// plane U vs V points
TRIANGLE_PLANE(tu_vertices[0],tu_vertices[1],tu_vertices[2],tu_plane);
dv[0] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[0]);
dv[1] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[1]);
dv[2] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[2]);
dv0dv1 = dv[0] * dv[1];
dv0dv2 = dv[0] * dv[2];
if(dv0dv1>0.0f && dv0dv2>0.0f) // same sign on all of them + not equal 0 ?
{
if(dv[0]<0) //we need test behind the triangle plane
{
distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]);
distances[1] = -distances[1];
if(distances[1]>margin) return false; //never intersect
//reorder triangle u
VEC_SWAP(tu_vertices[0],tu_vertices[1]);
VEC_SCALE_4(tu_plane,-1.0f,tu_plane);
}
else
{
distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]);
if(distances[1]>margin) return false; //never intersect
}
}
else
{
//Look if we need to invert the triangle
distances[1] = (dv[0]+dv[1]+dv[2])/3.0f; //centroid
if(distances[1]<0.0f)
{
//reorder triangle v
VEC_SWAP(tu_vertices[0],tu_vertices[1]);
VEC_SCALE_4(tu_plane,-1.0f,tu_plane);
distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]);
distances[1] = -distances[1];
}
else
{
distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]);
}
}
GUINT bl;
/* bl = cross_line_intersection_test();
if(bl==3)
{
//take edge direction too
bl = distances.maxAxis();
}
else
{*/
bl = 0;
if(distances[0]<distances[1]) bl = 1;
//}
if(bl==2) //edge edge separation
{
if(distances[2]>margin) return false;
contacts.m_penetration_depth = -distances[2] + margin;
contacts.m_points[0] = closest_point_v;
contacts.m_point_count = 1;
VEC_COPY(contacts.m_separating_normal,edge_edge_dir);
return true;
}
//clip face against other
GUINT point_count;
//TODO
if(bl == 0) //clip U points against V
{
point_count = clip_triangle(tv_plane,tv_vertices,tu_vertices,contact_points);
if(point_count == 0) return false;
contacts.merge_points(tv_plane,margin,contact_points,point_count);
}
else //clip V points against U
{
point_count = clip_triangle(tu_plane,tu_vertices,tv_vertices,contact_points);
if(point_count == 0) return false;
contacts.merge_points(tu_plane,margin,contact_points,point_count);
contacts.m_separating_normal *= -1.f;
}
if(contacts.m_point_count == 0) return false;
return true;
}
