n_3d.c

Go to the documentation of this file.

 
#include "nilorea/n_3d.h"
#include "math.h"
 
 
double distance( VECTOR3D *p1, VECTOR3D *p2 )
{
    return sqrt( ( (*p1)[ 0 ] - (*p2)[ 0 ] ) * ( (*p1)[ 0 ] - (*p2)[ 0 ] ) +
                 ( (*p1)[ 1 ] - (*p2)[ 1 ] ) * ( (*p1)[ 1 ] - (*p2)[ 1 ] ) +
                 ( (*p1)[ 2 ] - (*p2)[ 2 ] ) * ( (*p1)[ 2 ] - (*p2)[ 2 ] ) );
} /* distance(...) */
 
 
 
int update_physics_position_nb( PHYSICS *object, int it, double delta_t )
{
    __n_assert( object, return FALSE );
 
    object -> speed[ it ] = object -> speed[ it ] + ( object -> acceleration[ it ] * delta_t )/ 1000000.0 ;
    object -> position[ it ] = object -> position[ it ] + ( object -> speed[ it ] * delta_t ) / 1000000.0  + ( object -> acceleration[ it ] * (delta_t / 1000000.0 ) * ( delta_t / 1000000.0 ) ) / 2.0 ;
    object -> angular_speed[ it ] = object -> angular_speed[ it ] + ( object -> angular_acceleration[ it ] * delta_t ) / 1000000.0 ;
    object -> speed[ it ] = object -> speed[ it ] + ( object -> gravity[ it ] * delta_t ) / 1000000.0 ;
 
    return TRUE ;
} /* update_physics_position_nb(...) */
 
 
 
int update_physics_position_reverse_nb( PHYSICS *object, int it, double delta_t )
{
    __n_assert( object, return FALSE );
 
    object -> position[ it ] = object -> position[ it ] - ( ( object -> speed[ it ] * delta_t ) / 1000000.0  + ( object -> acceleration[ it ] * (delta_t/1000000.0) * (delta_t/1000000.0 ) ) / 2.0 );
    object -> speed[ it ] = object -> speed[ it ] - ( object -> acceleration[ it ] * delta_t )/ 1000000.0 ;
    object -> angular_speed[ it ] = object -> angular_speed[ it ] - ( object -> angular_acceleration[ it ] * delta_t ) / 1000000.0 ;
 
    return TRUE ;
} /* update_physics_position_reverse_nb */
 
 
int update_physics_position_reverse( PHYSICS *object, double delta_t )
{
    for( int it = 0 ; it < 3 ; it ++ )
    {
        object -> speed[ it ] = -object -> speed[ it ] ;
        object -> acceleration[ it ] = -object -> acceleration[ it ] ;
        object -> angular_speed[ it ] = -object -> angular_speed[ it ];
 
        update_physics_position_nb( object, it, delta_t );
 
        object -> speed[ it ] = -object -> speed[ it ] ;
        object -> acceleration[ it ] = -object -> acceleration[ it ] ;
        object -> angular_speed[ it ] = -object -> angular_speed[ it ] ;
    }
    return TRUE ;
} /* update_physics_position_reverse(...) */
 
 
 
int update_physics_position( PHYSICS *object, double delta_t )
{
    object -> delta_t = delta_t ;
    for( int it = 0 ; it < 3 ; it ++ )
    {
        update_physics_position_nb( object, it, delta_t );
    }
    return TRUE ;
}
 
 
 
static int same_sign( double a, double b )
{
    return (( a * b) >= 0 );
} /* same_sign(...) */
 
 
 
int vector_intersect(  VECTOR3D *p1, VECTOR3D *p2, VECTOR3D *p3, VECTOR3D *p4, VECTOR3D *px )
{
    double a1 = 0, a2 = 0, b1 = 0, b2 = 0, c1 = 0, c2 = 0,
           r1 = 0, r2 = 0, r3 = 0, r4 = 0,
           denom = 0, offset = 0, num = 0 ;
 
    /* Compute a1, b1, c1, where line joining points 1 and 2 is
       "a1 x + b1 y + c1 = 0". */
    a1 = (*p2)[ 1 ] - (*p1)[ 1 ];
    b1 = (*p1)[ 0 ] - (*p2)[ 0 ];
    c1 = ((*p2)[ 0 ] * (*p1)[ 1 ]) - ((*p1)[ 0 ] * (*p2)[ 1 ]);
 
    /* Compute r3 and r4. */
    r3 = ((a1 * (*p3)[ 0 ]) + (b1 * (*p3)[ 1 ]) + c1);
    r4 = ((a1 * (*p4)[ 0 ]) + (b1 * (*p4)[ 1 ]) + c1);
 
    /* Check signs of r3 and r4. If both point 3 and point 4 lie on
       same side of line 1, the line segments do not intersect. */
    if((r3 != 0) && (r4 != 0) && same_sign(r3, r4))
    {
        return VECTOR3D_DONT_INTERSECT;
    }
 
    /* Compute a2, b2, c2 */
    a2 = (*p4)[ 1 ] - (*p3)[ 1 ];
    b2 = (*p3)[ 0 ] - (*p4)[ 0 ];
    c2 = ((*p4)[ 0 ] * (*p3)[ 1 ]) - ((*p3)[ 0 ] * (*p4)[ 1 ]);
 
    /* Compute r1 and r2 */
    r1 = (a2 * (*p1)[ 0 ]) + (b2 * (*p1)[ 1 ]) + c2;
    r2 = (a2 * (*p2)[ 0 ]) + (b2 * (*p2)[ 1 ]) + c2;
 
    /* Check signs of r1 and r2. If both point 1 and point 2 lie
       on same side of second line segment, the line segments do
       not intersect. */
    if( (r1 != 0) && (r2 != 0) && same_sign(r1, r2) )
    {
        return VECTOR3D_DONT_INTERSECT;
    }
 
    /* Line segments intersect: compute intersection point. */
    denom = (a1 * b2) - (a2 * b1);
 
    if( denom == 0 )
    {
        return VECTOR3D_COLLINEAR;
    }
 
    if( denom < 0 )
    {
        offset = -denom / 2;
    }
    else
    {
        offset = denom / 2 ;
    }
 
    /* The denom/2 is to get rounding instead of truncating. It
       is added or subtracted to the numerator, depending upon the
       sign of the numerator. */
    num = (b1 * c2) - (b2 * c1);
    if(num < 0)
    {
        (*px)[ 0 ] = (num - offset) / denom;
    }
    else
    {
        (*px)[ 0 ] = (num + offset) / denom;
    }
 
    num = (a2 * c1) - (a1 * c2);
    if(num < 0)
    {
        (*px)[ 1 ] = ( num - offset) / denom;
    }
    else
    {
        (*px)[ 1 ] = (num + offset) / denom;
    }
 
    /* lines_intersect */
    return VECTOR3D_DO_INTERSECT;
} /* vector_intersect(...) */
 
 
 
double vector_dot_product( VECTOR3D *vec1, VECTOR3D *vec2 )
{
    return (*vec1)[ 0 ] * (*vec2)[ 0 ] + (*vec1)[ 1 ] * (*vec2)[ 1 ] + (*vec1)[ 2 ] * (*vec2)[ 2 ];
} /* vector_dot_product(...) */
 
 
 
double vector_normalize( VECTOR3D *vec )
{
    double res = 0.0 ;
    for( int i=0 ; i < 3 ; i++ )
    {
        res += pow( (*vec)[ i ], 2 );
    }
    return sqrt( res );
} /* vector_normalize(...) */
 
 
 
double vector_angle_between( VECTOR3D *vec1, VECTOR3D *vec2 )
{
    return acos( vector_dot_product( &(*vec1), &(*vec2) ) / ( vector_normalize( &(*vec1) ) * vector_normalize( &(*vec2) ) ) );
} /* vector_angle_between( ... ) */