void gim_merge_contacts_unique(GDYNAMIC_ARRAY * source_contacts,
					GDYNAMIC_ARRAY * dest_contacts)
{
    dest_contacts->m_size = 0;
    //Traverse the source contacts
	GUINT32 source_count = source_contacts->m_size;
	if(source_count==0) return;

	GIM_CONTACT * psource_contacts	= GIM_DYNARRAY_POINTER(GIM_CONTACT,(*source_contacts));

	//add the unique contact
	GIM_CONTACT * pcontact = 0;
    GIM_DYNARRAY_PUSH_EMPTY(GIM_CONTACT,(*dest_contacts));
    pcontact = GIM_DYNARRAY_POINTER_LAST(GIM_CONTACT,(*dest_contacts));
    //set the first contact
    GIM_COPY_CONTACTS(pcontact, psource_contacts);

    if(source_count==1) return;
    //scale the first contact
    VEC_SCALE(pcontact->m_normal,pcontact->m_depth,pcontact->m_normal);

    psource_contacts++;

	//Average the contacts
    GUINT32 i;
	for(i=1;i<source_count;i++)
	{
	    VEC_SUM(pcontact->m_point,pcontact->m_point,psource_contacts->m_point);
	    VEC_ACCUM(pcontact->m_normal,psource_contacts->m_depth,psource_contacts->m_normal);
	    psource_contacts++;
	}

	GREAL divide_average = 1.0f/((GREAL)source_count);

	VEC_SCALE(pcontact->m_point,divide_average,pcontact->m_point);

	pcontact->m_depth = VEC_DOT(pcontact->m_normal,pcontact->m_normal)*divide_average;
	GIM_SQRT(pcontact->m_depth,pcontact->m_depth);

	VEC_NORMALIZE(pcontact->m_normal);

	/*GREAL normal_len;
    VEC_INV_LENGTH(pcontact->m_normal,normal_len);
	VEC_SCALE(pcontact->m_normal,normal_len,pcontact->m_normal);

    //Deep = LEN(normal)/SQRT(source_count)
    GIM_SQRT(divide_average,divide_average);
	pcontact->m_depth = divide_average/normal_len;
	*/
}
/*!

\param triangle
\param s1
\param s2
\param contacts Contains the closest points on the segment (1,2), and the normal points to segment, and m_depth contains the distance

\post The contacts array is not set to 0. It adds aditional contacts
*/
void gim_closest_point_triangle_segment(GIM_TRIANGLE_DATA * triangle, vec3f s1,vec3f s2, GDYNAMIC_ARRAY * contacts)
{
    vec3f segment_points[4];
    vec3f closest_points[2];
    GUINT intersection_type, out_edge= 10;
    GREAL dis, dis_temp,perpend;
    vec4f sdiff;

    dis = DISTANCE_PLANE_POINT(triangle->m_planes.m_planes[0],s1);
    dis_temp = DISTANCE_PLANE_POINT(triangle->m_planes.m_planes[0],s2);

    if(dis<=0.0f && dis_temp<=0.0f) return;

    VEC_DIFF(sdiff,s2,s1);
    perpend = VEC_DOT(sdiff,triangle->m_planes.m_planes[0]);

    if(!IS_ZERO(perpend)) // Not perpendicular
    {
        if(dis<dis_temp)
        {
            VEC_COPY(closest_points[0],s1);
        }
        else
        {
            dis = dis_temp;
            VEC_COPY(closest_points[0],s2);
        }

        //Testing segment vertices over triangle
        if(dis>=0.0f && dis_temp>=0.0f)
        {
            POINT_IN_HULL(closest_points[0],(&triangle->m_planes.m_planes[1]),3,out_edge);

            if(out_edge==0)//Point over face
            {
                GIM_PUSH_CONTACT((*contacts),closest_points[0] ,triangle->m_planes.m_planes[0] ,dis,0, 0, 0,0);
                return;
            }
        }
        else
        {

            PLANE_CLIP_SEGMENT(s1,s2,triangle->m_planes.m_planes[0],closest_points[1]);

            POINT_IN_HULL(closest_points[1],(&triangle->m_planes.m_planes[1]),3,out_edge);

            if(out_edge==0)//Point over face
            {
                GIM_PUSH_CONTACT((*contacts),closest_points[0] ,triangle->m_planes.m_planes[0] ,dis,0, 0, 0,0);
                return;
            }
        }

    }
    else // Perpendicular Face
    {
        //out_edge=10
        //Clip segment by triangle
    //    Edge1
        PLANE_CLIP_SEGMENT_CLOSEST(s1,s2,triangle->m_planes.m_planes[1],segment_points[0],segment_points[1],intersection_type);
        if(intersection_type==0||intersection_type==1)
        {
            out_edge = 0;
            VEC_COPY(closest_points[0],segment_points[0]);
        }
        else
        {
            //Edge2
            PLANE_CLIP_SEGMENT_CLOSEST(segment_points[0],segment_points[1],triangle->m_planes.m_planes[2],segment_points[2],segment_points[3],intersection_type);
            if(intersection_type==0||intersection_type==1)
            {
                out_edge = 1;
                VEC_COPY(closest_points[0],segment_points[3]);
            }
            else
            {
                //Edge3
                PLANE_CLIP_SEGMENT_CLOSEST(segment_points[2],segment_points[3],triangle->m_planes.m_planes[3],closest_points[0],closest_points[1],intersection_type);
                if(intersection_type==0||intersection_type==1)
                {
                    out_edge = 2;
                }
            }
        }
        //POST closest_points[0] and closest_points[1] are inside the triangle, if out_edge>2
        if(out_edge>2) // Over triangle
        {
            dis = VEC_DOT(closest_points[0],triangle->m_planes.m_planes[0]);
            GIM_PUSH_CONTACT((*contacts),closest_points[0] ,triangle->m_planes.m_planes[0] ,dis,0, 0, 0,0);
            GIM_PUSH_CONTACT((*contacts),closest_points[1] ,triangle->m_planes.m_planes[0] ,dis,0, 0, 0,0);
            return;
        }
    }

    //Find closest edges
    out_edge = 10;
    dis = G_REAL_INFINITY;
    GUINT i;
    for(i=0;i<3;i++)
    {
        SEGMENT_COLLISION(s1,s2,triangle->m_vertices[i],triangle->m_vertices[(i+1)%3],segment_points[0],segment_points[1]);
        VEC_DIFF(sdiff,segment_points[0],segment_points[1]);
        dis_temp = VEC_DOT(sdiff,sdiff);
        if(dis_temp< dis)
        {
            dis = dis_temp;
            out_edge = i;
            VEC_COPY(closest_points[0],segment_points[0]);
            VEC_COPY(closest_points[1],sdiff);//normal
        }
    }
    if(out_edge>2) return ;// ???? ASSERT this please

    if(IS_ZERO(dis))
    {
        //Set face plane
        GIM_PUSH_CONTACT((*contacts),closest_points[0] ,triangle->m_planes.m_planes[0] ,0.0f,0, 0, 0,0);

    }
    else
    {
        GIM_SQRT(dis,dis);
        VEC_SCALE(closest_points[1],(1.0f/dis),closest_points[1]);//normal
        GIM_PUSH_CONTACT((*contacts),closest_points[0] ,closest_points[1],dis,0, 0, 0,0);
    }
}
Пример #3
0
GREAL gim_sqrt(GREAL f)
{
    GREAL r;
    GIM_SQRT(f,r);
    return r;
}