ivp_tetra_intrude.cxx

hl2 source code. Do not use it illegal.

// Copyright (C) Ipion Software GmbH 1999-2000. All rights reserved.



#include <ivp_physics.hxx>
#if defined(LINUX)
#	include <string.h>
#endif

#include <ivu_memory.hxx>
#include <ivp_object_polygon_tetra.hxx>

#include <ivu_hash.hxx>
#include <ivp_tetra_intrude.hxx>

IVP_Tri_Edge *IVP_Tri_Edge::search_nearest_edge_to(IVP_U_Point *reference, IVP_DOUBLE *quad_dist) {
    // searches the edge around this, must be less than dist
    // 4 Physik

    IVP_Tri_Edge *fin = this->opposite;
    IVP_Tri_Edge *tmp_edge = this->opposite->next->opposite;
    IVP_Tri_Edge *shrt = this;
    IVP_DOUBLE tdist;
    while (tmp_edge != fin) {
	tdist = reference->quad_distance_to(tmp_edge->start_point);
	if (tdist < *quad_dist) {
	    *quad_dist = tdist;
	    shrt = tmp_edge;
	}
	tmp_edge = tmp_edge->next->opposite;
    }
    return shrt;
}


void IVP_Tetra_Point::print(const char *text){
    if (!text) text = "";
    printf("%s	Tetra-Point: Mask %X  Coverbits %X %X %X  pntnum=%i\n",
	   text, this->tmp_side_of_triangle_bits,
	   cover_area_bits[0],cover_area_bits[1],cover_area_bits[2],
	   opoint->point_num());
}

int IVP_Tetra_Point::p(){
    this->print(0);
    return 0;
}

void IVP_Tetra_Edge::print(const char *text){
    if (!text) text = "";
    printf("%s	Tetra_Edge: Bits %X %X %X\n",
	   text,cover_area_bits[0],cover_area_bits[1],cover_area_bits[2]);
    tetra_points[0]->print("		");
    tetra_points[1]->print("		");
}

int IVP_Tetra_Edge::p(){
    this->print(0);
    return 0;
}


void IVP_Intrusion_Included_Points::print(const char *text){
    if (!text) text = "";
    printf("%s	Intrusion_Point",text);
    tetra_point->print("	");
}

int IVP_Intrusion_Included_Points::p(){
    this->print(0);
    return 0;
}

void IVP_Intrusion_Intersection::print(const char *text){
    if (!text){
	text = "";
    }else{
	if ( long(pairing_intersection) > long(this)) return;
    }
    printf("%s %s Intersection pnt %i - %i",text,
	   (type != IVP_INTRUSION_CHECK_OVERLAP)? "normal":"equalp",
	   line_endpoints[0]->point_num(),
	   line_endpoints[1]->point_num());
    tri_edge->print("	TriEdge:");
    if (included_point){
	included_point->print("	Included Point\n");
    }
    if (pairing_intersection){
	pairing_intersection->print("Pairing Intersection:");
    }
}

int IVP_Intrusion_Intersection::p(){
    this->print(0);
    return 0;
}


void IVP_Intrusion_Status::print(const char *text){
    if (!text) text = "";
    printf("%s	Intrusion Status\n",text);
    IVP_Intrusion_Included_Points *p;
    for (p=intruded_points; p; p=p->next){
	p->print("	");
    }
    IVP_Intrusion_Intersection *s;
    for (s=intersections; s ; s=s->next){
	s->print("	");
    }
}

    
int IVP_Intrusion_Status::p(){
    this->print(0);
    return 0;
}

IVP_Intrusion_Status::IVP_Intrusion_Status(){
    P_MEM_CLEAR(this);
}

IVP_Intrusion_Status::~IVP_Intrusion_Status(){
    IVP_Intrusion_Included_Points *p,*pn;
    for (p=intruded_points; p; p=pn){
	pn = p->next;
	delete p;
    }
    IVP_Intrusion_Intersection *s,*sn;
    for (s=intersections; s ; s=sn){
	sn = s->next;
	delete s;
    }
}

void IVP_Tetra_Point::init(IVP_Tetra_Intrude *  ){
    ;
}

IVP_Tetra_Intrude::IVP_Tetra_Intrude(IVP_Tetra_Point *i_tetra_points, int orig_pnt_anz){
    P_MEM_CLEAR(this);
    twop_2_tetra_edge_hash= new IVP_Hash(orig_pnt_anz*2,
				       sizeof(IVP_Tetra_Edge *)*2,
				       (void *)P_INVALID_TETRA_EDGE);
    tetra_points = i_tetra_points;
    n_tetra_points = orig_pnt_anz;
    memsize_of_tetra_edges = n_tetra_points * 10;
    n_tetra_edges = 0;
    tetra_edges = (IVP_Tetra_Edge *)p_calloc(memsize_of_tetra_edges,sizeof(IVP_Tetra_Edge));
    {	// search min max extends of opoints
	min_koord.set(tetra_points[0].opoint);
	max_koord.set(tetra_points[0].opoint);
	int i;
	IVP_Tetra_Point *p = &tetra_points[0];
	for (i=orig_pnt_anz-1;i>=0;i--,p++){
	    min_koord.line_min(p->opoint);
	    max_koord.line_max(p->opoint);
	}
    }
    {
    	int i;
	IVP_Tetra_Point *p = &tetra_points[0];
	for (i=orig_pnt_anz-1;i>=0;i--,p++){
	    p->init(this);
	}
    }
}

IVP_Tetra_Intrude::~IVP_Tetra_Intrude(){
    delete twop_2_tetra_edge_hash;
    P_FREE(tetra_edges);
}


int IVP_Tetra_Intrude::pe(const int pnt_num){// print all edges including pnt_num
    int i;
    for (i=0;i<n_tetra_edges;i++){
	if ( pnt_num == tetra_edges[i].tetra_points[0]->opoint->point_num()){
	    tetra_edges[i].print("");
	}
 	if ( pnt_num == tetra_edges[i].tetra_points[1]->opoint->point_num()){
	    tetra_edges[i].print("");
	}
    }
    return 0;
}

inline IVP_DOUBLE p_quad_distance_between_two_straights(IVP_U_Point *linea_0,IVP_U_Point *linea_1,IVP_U_Point *lineb_0, IVP_U_Point *lineb_1){
    IVP_U_Hesse hesse;
    IVP_U_Point linea_2;linea_2.subtract(lineb_1,lineb_0);
    hesse.calc_hesse(linea_0, linea_1, &linea_2);
    hesse.normize();
    IVP_DOUBLE dist = hesse.get_dist(lineb_0);
    return IVP_Inline_Math::fabsd(dist);
}

void IVP_Tetra_Intrude::init_tetra_edge(IVP_Tetra_Edge *edge, IVP_Tetra_Point *p0, IVP_Tetra_Point *p1,IVP_Tri_Edge *e){
    P_MEM_CLEAR(edge);
    if (p0<p1){
	edge->tetra_points[0] = p0;
	edge->tetra_points[1] = p1;
    }else{
	edge->tetra_points[1] = p0;
	edge->tetra_points[0] = p1;
    }
    edge->source_tri_edge = e;

    IVP_U_Point min,max;
    min.set(p0->opoint);
    max.set(p0->opoint);
    min.line_min(p1->opoint);
    max.line_max(p1->opoint);
    this->point_2_bits(&min,&max,edge->cover_area_bits);

    for (int i=2;i>=0;i--){
	p0->cover_area_bits[i] |= edge->cover_area_bits[i];
	p1->cover_area_bits[i] |= edge->cover_area_bits[i];
    }
}

void IVP_Tetra_Intrude::point_2_bits(IVP_U_Point *lpos,IVP_U_Point *rpos,int *result_bitmasp){
    int k;
    
    for (k=0;k<3;k++){
	int bitmask = 0;
	IVP_DOUBLE inv_nen = 31.999f / (max_koord.k[k] - min_koord.k[k]);
	int l = (int)((lpos->k[k] - min_koord.k[k] - P_Pop_Eps) * inv_nen);
	int r = (int)((rpos->k[k] - min_koord.k[k] + P_Pop_Eps) * inv_nen);
	IVP_ASSERT(l<=r);
	for(;l<=r;l++){
	    bitmask |= 1<<l;
	}
	result_bitmasp[k] = bitmask;
    }
}

void IVP_Tetra_Intrude::checkin_edge(IVP_Tri_Edge *edge){
    if (edge->tmp.gen.checked_in) return; // already check in
    char *te = 0;
    IVP_Tetra_Edge *t_edge;
    IVP_Tetra_Point *buffer[2];
    if (edge->tmp.gen.tetra_point < edge->next->tmp.gen.tetra_point){
	buffer[0] = edge->tmp.gen.tetra_point;
	buffer[1] = edge->next->tmp.gen.tetra_point;
    }else{
	buffer[1] = edge->tmp.gen.tetra_point;
	buffer[0] = edge->next->tmp.gen.tetra_point;
    }

    te = (char *)twop_2_tetra_edge_hash->find((char *)&buffer[0]);

    if (te == P_INVALID_TETRA_EDGE){
	if (n_tetra_edges == memsize_of_tetra_edges){
	    memsize_of_tetra_edges = 3*memsize_of_tetra_edges/2;
	    //printf("*********** Increasing tetra_edge memory *****\n");
	    IVP_Tetra_Edge *newmem =	(IVP_Tetra_Edge *)p_calloc(
		    memsize_of_tetra_edges,sizeof(IVP_Tetra_Edge));
	    memcpy( (char *)newmem, (char *)tetra_edges, sizeof(IVP_Tetra_Edge)*n_tetra_edges);
	    delete tetra_edges;
	    tetra_edges = newmem;
	}
	t_edge = &tetra_edges[n_tetra_edges++];
	twop_2_tetra_edge_hash->add((char *)&buffer[0],
				    (void *)(((char *)t_edge)-((char *)tetra_edges)));

	this->init_tetra_edge(t_edge,edge->tmp.gen.tetra_point,edge->next->tmp.gen.tetra_point,edge);
    }else{
	t_edge = (IVP_Tetra_Edge *)(te + long(tetra_edges));
    }
    t_edge->reference_count++;
    edge->tmp.gen.checked_in = IVP_TRUE;
}

void IVP_Tetra_Intrude::checkout_edge(IVP_Tri_Edge *edge){
    if (!edge->tmp.gen.checked_in) return; // already check in
    edge->tmp.gen.checked_in = IVP_FALSE;
    char *te = 0;
    IVP_Tetra_Edge *t_edge;

    IVP_Tetra_Point *buffer[2];
    if (edge->tmp.gen.tetra_point < edge->next->tmp.gen.tetra_point){
	buffer[0] = edge->tmp.gen.tetra_point;
	buffer[1] = edge->next->tmp.gen.tetra_point;
    }else{
	buffer[1] = edge->tmp.gen.tetra_point;
	buffer[0] = edge->next->tmp.gen.tetra_point;
    }

    te = (char *)twop_2_tetra_edge_hash->find((char *)&buffer[0]);
    t_edge = (IVP_Tetra_Edge *)(te + long(tetra_edges));
    t_edge->reference_count--;
    /* edge no more referenced -> delete it */
    if (t_edge->reference_count<=0){
	twop_2_tetra_edge_hash->remove((char *)&t_edge->tetra_points[0]);
       	IVP_Tetra_Edge *last_edge = &tetra_edges[n_tetra_edges-1];
	if (last_edge != t_edge){
	    twop_2_tetra_edge_hash->remove((char *)&last_edge->tetra_points[0]);
	    twop_2_tetra_edge_hash->add((char *)&last_edge->tetra_points[0],
				    (void *)(((char *)t_edge)-((char *)tetra_edges)));
	    *t_edge = *last_edge;
	}
	n_tetra_edges --;
    }
}

IVP_INTRUSION_CHECK_RESULTS P_THREE_CHECK_INTRUDE(int me, int other0, int other1, int other2,
						       IVP_Tetra_Point *t0,IVP_Tetra_Point *t1,
						       IVP_Tri_Edge *tri_edges[4],
						       IVP_U_Hesse hesse_of_t[4],
						       IVP_U_Hesse edge_hesses[6],
						       IVP_Tri_Edge *edge_edges[6],
						       IVP_DOUBLE	pop_eps_of_t[4],
						       IVP_U_Point &intrude_position,
						       IVP_Intrusion_Status *status){
    int bor = t0->tmp_side_of_triangle_bits | t1->tmp_side_of_triangle_bits;
    int bitsand = t0->tmp_side_of_triangle_bits	& t1->tmp_side_of_triangle_bits;
    
    IVP_INTRUSION_CHECK_RESULTS type = IVP_INTRUSION_CHECK_LINE;
    while (1){
	if ( ((bitsand & 0xf & ~(bitsand >> 8)) & (1<<me)) != 0) return IVP_INTRUSION_CHECK_NONE; // both both inside and not on the surface

	if ( (bor  & (0x1111<<me)) == (0x11<<me) ){/* real intrusion, no epsilon, no identic to point ->