@@ -3455,152 +3455,61 @@ int sample_mesh_intersect(double *t,int *num_solutions,double *r,double *v,struc
34553455 Coords edge1 ,edge2 ,h ,s ,q ,tmp ,intersect_pos ;
34563456 double UNION_EPSILON = 0.0000001 ;
34573457 double this_facet_t ;
3458- double a ,f ,u ,V , t_intersect [ n_facets ] ;
3459- * num_solutions = 0 ;
3460- for ( iter = 0 ; iter < n_facets ; iter ++ ) {
3461- /*////printf("\n\n facet v1 = [%f,%f,%f]",v1_x[iter],v1_y[iter],v1_z[iter] );
3462- ////printf("\n facet v2 = [%f,%f,%f]",v2_x[iter],v2_y[iter],v2_z[iter]) ;
3463- ////printf("\n facet v3 = [%f,%f,%f]",v3_x[ iter],v3_y[ iter],v3_z[iter]);*/
3458+ double a ,f ,u ,V ;
3459+ double * t_intersect ;
3460+ if ( n_facets ) {
3461+ t_intersect = malloc ( n_facets * sizeof ( double ) );
3462+ * num_solutions = 0 ;
3463+ for ( iter = 0 ; iter < n_facets ; iter ++ ){
34643464 // Intersection with face plane (Möller–Trumbore)
34653465 edge1 = coords_set (* (v2_x + iter )- * (v1_x + iter ),* (v2_y + iter )- * (v1_y + iter ),* (v2_z + iter )- * (v1_z + iter ));
3466- ////printf("\n edge 1 = [%f,%f,%f]",edge1.x,edge1.y,edge1.z);
3467- edge2 = coords_set (* (v3_x + iter )- * (v1_x + iter ),* (v3_y + iter )- * (v1_y + iter ),* (v3_z + iter )- * (v1_z + iter ));
3468- //h = vec_prod(rotated_velocity,edge2);
3469- vec_prod (h .x ,h .y ,h .z ,rotated_velocity .x ,rotated_velocity .y ,rotated_velocity .z ,edge2 .x ,edge2 .y ,edge2 .z );
3470- ////printf("\n h = [%f,%f,%f]",h.x,h.y,h.z);
3471- ////printf("\n rotated_velocity = [%f,%f,%f]",rotated_velocity.x,rotated_velocity.y,rotated_velocity.z);
3472- ////printf("\n edge2 = [%f,%f,%f]",edge2.x,edge2.y,edge2.z);
3473- //h = coord_set(rotated_velocity.y*edge2.z-rotated_velocity.z*edge2.y, rotated_velocity.z*edge2.x-rotated_velocity.x*edge2.z, rotated_velocity.x*edge2.y-rotated_velocity.y*edge2.x);
3474- //a = Dot(h,edge1);
3466+ edge2 = coords_set (* (v3_x + iter )- * (v1_x + iter ),* (v3_y + iter )- * (v1_y + iter ),* (v3_z + iter )- * (v1_z + iter ));
3467+ vec_prod (h .x ,h .y ,h .z ,rotated_velocity .x ,rotated_velocity .y ,rotated_velocity .z ,edge2 .x ,edge2 .y ,edge2 .z );
3468+
34753469 a = Dot (edge1 ,h );
3476- ////printf("\n a=%f",a);
3477- //if (a > -UNION_EPSILON && a < UNION_EPSILON){
3478- ////printf("\n UNION_EPSILON fail");
3479- //} else{
3480- f = 1.0 /a ;
3481- s = coords_sub (rotated_coordinates , coords_set (* (v1_x + iter ),* (v1_y + iter ),* (v1_z + iter )));
3482- u = f * (Dot (s ,h ));
3483- if (u < 0.0 || u > 1.0 ){
3484- ////printf("\n Nope 1");
3485- }else {
3486- //q = vec_prod(s,edge1);
3487- vec_prod (q .x ,q .y ,q .z ,s .x ,s .y ,s .z ,edge1 .x ,edge1 .y ,edge1 .z );
3488- V = f * Dot (rotated_velocity ,q );
3489- if (V < 0.0 || u + V > 1.0 ){
3490- ////printf("\n Nope 2");
3491- } else {
3492- // At this stage we can compute t to find out where the intersection point is on the line.
3493- //tmp = Dot(q,edge2)
3494- ////printf("\nt inside loop = %f",f* Dot(q,edge2));
3495- if (f * Dot (q ,edge2 ) > 0 ){
3496-
3497- t_intersect [counter ] = f * Dot (q ,edge2 );
3498- //printf("\nIntersects at time: t= %f\n",t_intersect[counter] );
3499- counter ++ ;
3500-
3501- //intersect_pos = coords_set(rotated_coordinates.x+t_intersect[counter]*rotated_velocity.x,rotated_coordinates.y+t_intersect[counter]*rotated_velocity.y,rotated_coordinates.z+t_intersect[counter]*rotated_velocity.z);
3502- //////printf("\n intersects at [%f,%f,%f]",intersect_pos.x,intersect_pos.y,intersect_pos.z);
3503- }
3504-
3505-
3506-
3507- }
3508- }
3509- //}
3510- }
3511-
3512- // find two smallest non-zero intersections:
3513- /*
3514- double t_min = -1.0;
3515- double t_second_min= -1.0;
3516- for (iter = 0 ; iter < counter; iter++){
3517- // test
3518- if (t_min == -1 && t_intersect[iter] > 0.0){
3519- t_min = t_intersect[iter];
3520-
3521- } else{
3522- if (t_intersect[iter] > 0.0 && t_intersect[iter] < t_min) {
3523- t_second_min = t_min;
3524- t_min = t_intersect[iter];
3525-
3526- }
3527- if (t_intersect[iter] > 0.0 && t_intersect[iter] > t_min) {
3528- if (t_intersect[iter] < t_second_min || t_second_min == -1.0){
3529- t_second_min = t_intersect[iter];
3530- }
3531- }
3532- }
3533-
3534-
3535- }
3536-
3537- //printf("\n number of intersections: %i\n",counter);
3538-
3539-
3540-
3541-
3542- if (t_second_min > 0) {
3543- if (counter % 2 == 0){
3544- t[0] = t_second_min;
3545- t[1] = t_min;
3546- *num_solutions = 2;
3547- //printf("\n t[0] = %f t[1] = %f \n",t_min,t_second_min);
3548- } else{
3549- t[0] = -1;
3550- t[1] = t_min;
3551- *num_solutions = 1;
3552- //printf("\n t[0] = %f t[1] = %f \n",t_min,t_second_min);
3553- }
3554- ////printf("\n t[0] = %f t[1] = %f \n",t[0],t[1]);
3555-
3556- //printf("\n num_solutions: %i\n",*num_solutions);
3557- return 1;
3558- }else if (t_min>0) {
3559- t[0] = t_min;
3560- t[1] = -1;
3561- *num_solutions = 1;
3562- //printf("\n num_solutions: %i\n",*num_solutions);
3563- return 1;
3564- } else {
3565- t[0] = -1;
3566- t[1] = -1;
3567- *num_solutions = 0;
3568- //printf("\n num_solutions: %i\n",*num_solutions);
3569- return 0;
3570- }
3571- return 0;
3572- */
3573-
3574-
3575- //for (iter=0;iter<99;iter++){
3576- // printf("\n t[%i] = %f",iter,t[iter]);
3577- // t[iter] = -1;
3578- //}
3470+ f = 1.0 /a ;
3471+ s = coords_sub (rotated_coordinates , coords_set (* (v1_x + iter ),* (v1_y + iter ),* (v1_z + iter )));
3472+ u = f * (Dot (s ,h ));
3473+ if (u < 0.0 || u > 1.0 ){
3474+ ////printf("\n Nope 1");
3475+ }else {
3476+ vec_prod (q .x ,q .y ,q .z ,s .x ,s .y ,s .z ,edge1 .x ,edge1 .y ,edge1 .z );
3477+ V = f * Dot (rotated_velocity ,q );
3478+ if (V < 0.0 || u + V > 1.0 ){
3479+ ////printf("\n Nope 2");
3480+ } else {
3481+ if (f * Dot (q ,edge2 ) > 0 ){
3482+ t_intersect [counter ] = f * Dot (q ,edge2 );
3483+
3484+ counter ++ ;
3485+ }
3486+ }
3487+ }
3488+ }
35793489
3580- // Return all t
3581- int counter2 = 0 ;
3582- * num_solutions = 0 ;
3583- for (iter = 0 ; iter < counter ; iter ++ ){
3490+ // Return all t
3491+ int counter2 = 0 ;
3492+ * num_solutions = 0 ;
3493+ for (iter = 0 ; iter < counter ; iter ++ ){
35843494 if (t_intersect [iter ] > 0.0 ){
3585- t [counter2 ] = t_intersect [iter ];
3586- counter2 ++ ;
3587- * num_solutions = counter2 ;
3495+ t [counter2 ] = t_intersect [iter ];
3496+ counter2 ++ ;
3497+ * num_solutions = counter2 ;
35883498 }
3589- }
3590- // Sort t:
3591-
3592- if ( * num_solutions == 0 ){
3499+ }
3500+ // Sort t:
3501+ if ( * num_solutions == 0 ){
3502+ free ( t_intersect );
35933503 return 0 ;
3504+ }
3505+ qsort (t ,* num_solutions ,sizeof (double ), Sample_compare_doubles );
3506+ free (t_intersect );
3507+ return 1 ;
3508+ } else {
3509+ return 0 ;
35943510 }
3595- qsort (t ,* num_solutions ,sizeof (double ), Sample_compare_doubles );
3596- if (* num_solutions > 2 ){
3597- //printf("\n T(0) = %f T(1) = %f T(2) = %f T(3) = %f T(4) = %f",t[0],t[1],t[2],t[3],t[4]);
3598- //printf("\n TEST");
3599- }
3600- return 1 ;
36013511};
36023512
3603-
36043513int r_within_box_advanced (Coords pos ,struct geometry_struct * geometry ) {
36053514 // Unpack parameters
36063515 double width1 = geometry -> geometry_parameters .p_box_storage -> x_width1 ;
0 commit comments