/* 
 * PPRacer 
 * Copyright (C) 2004-2005 Volker Stroebel <volker@planetpenguin.de>
 *
 * Copyright (C) 1999-2001 Jasmin F. Patry
 * 
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 */

#include "course_load.h"
#include "course_render.h"
#include "hier.h"
#include "hier_util.h"
#include "model_hndl.h"
#include "part_sys.h"
#include "phys_sim.h"
#include "nmrcl.h"
#include "game_config.h"
#include "course_mgr.h"
#include "stuff.h"


#include "ppgltk/audio/audio.h"
#include "ppgltk/alg/defs.h"

#include "loop.h"

#include "game_mgr.h"



/*
 * Constants
 */
 
extern terrain_tex_t terrain_texture[NUM_TERRAIN_TYPES];
extern unsigned int num_terrains;

/* Tux's mass (kg) */
#define TUX_MASS 20

/* Width of Tux (m) */
#define TUX_WIDTH 0.45

/* Minimum speed (m/s) */
#define MIN_TUX_SPEED 1.4

/* Minimum speed for friction to take effect (m/s) */
#define MIN_FRICTION_SPEED 2.8

/* Initial Tux speed (m/s) */
#define INIT_TUX_SPEED 3.0

/* Maximum frictional force */
#define MAX_FRICTIONAL_FORCE 800

/* Maximum angle by which frictional force is rotated when turning*/
#define MAX_TURN_ANGLE 45

/* Maximum turning force perpendicular to direction of movement */
#define MAX_TURN_PERPENDICULAR_FORCE 400

/* Maximum increase in friction when turning */
#define MAX_TURN_PENALTY 0.15

/* Force applied by Tux when braking (N) */
#define BRAKE_FORCE 200

/* Maximum roll angle (degrees) */
#define MAX_ROLL_ANGLE 30

/* Roll angle while braking (degrees) */
#define BRAKING_ROLL_ANGLE 55

/* Baseline friction coeff for rolling */
#define IDEAL_ROLL_FRIC_COEFF 0.35

/* Baseline speed for rolling (m/s) */
#define IDEAL_ROLL_SPEED 6.0

/* Tux's orientation is updated using a first-order filter; these are the 
   time constants of the filter when tux is on ground and airborne (s) */
#define TUX_ORIENTATION_TIME_CONSTANT 0.14
#define TUX_ORIENTATION_AIRBORNE_TIME_CONSTANT 0.5

/* Max particles generated by turning (particles/s) */
#define MAX_TURN_PARTICLES 1500

/* Max particles generated by rolling (particles/s) */
#define MAX_ROLL_PARTICLES 3000

/* Particles generated by braking (particles/s) */
#define BRAKE_PARTICLES 4000

/* Number of particles is multiplied by ( speed / PARTICLES_SPEED_FACTOR ) */
#define PARTICLE_SPEED_FACTOR 40

/* Maximum deflection angle for particles (out to side of player)  */
#define MAX_PARTICLE_ANGLE 80

/* Speed at which maximum deflection occurs */
#define MAX_PARTICLE_ANGLE_SPEED 50

/* Particle speed = min ( MAX_PARTICLE_SPEED, speed*PARTICLE_SPEED_MULTIPLIER )
 */
#define PARTICLE_SPEED_MULTIPLIER 0.3
#define MAX_PARTICLE_SPEED 2

/* The compressiblity of the first section of the spring modelling Tux's
   tush (m) */
#define TUX_GLUTE_STAGE_1_COMPRESSIBILITY 0.05

/* The spring coefficient of first section of aforementioned tush (N/m) */
#define TUX_GLUTE_STAGE_1_SPRING_COEFF 1500

/* The damping coefficient of first section of aforementioned tush (N*s/m) */
#define TUX_GLUTE_STAGE_1_DAMPING_COEFF 100

/* The compressiblity of the second section of the spring modelling Tux's
   tush (m) */
#define TUX_GLUTE_STAGE_2_COMPRESSIBILITY 0.12

/* The spring coefficient of second section of aforementioned tush (N/m) */
#define TUX_GLUTE_STAGE_2_SPRING_COEFF 3000

/* The damping coefficient of second section of aforementioned tush (N*s/m) */
#define TUX_GLUTE_STAGE_2_DAMPING_COEFF 500

/* The spring coefficient of third section of aforementioned tush (N/m) */
#define TUX_GLUTE_STAGE_3_SPRING_COEFF 10000

/* The damping coefficient of third section of aforementioned tush (N*s/m) */
#define TUX_GLUTE_STAGE_3_DAMPING_COEFF 1000

/* The maximum force exertedd by both sections of spring (N) */
#define TUX_GLUTE_MAX_SPRING_FORCE 3000

/* Maximum distance that player can penetrate into terrain (m) */
#define MAX_SURFACE_PENETRATION 0.2

/* Density of air @ -3 C (kg/m^3) */
#define AIR_DENSITY 1.308

/* Diameter of sphere used to model Tux for air resistance calcs (m) */
#define TUX_DIAMETER TUX_WIDTH

/* Viscosity of air @ -3 C (N*s/m^2) */
#define AIR_VISCOSITY 17.00e-6

/* Drag coefficient vs. Reynolds number table (from Janna, _Introduction to
   Fluid Mechanics_, 3rd Edition, PWS-Kent, p 308) 
   Values are for Re in the range of 10^-1 to 10^6, spaced logarithmically
   (all values are log base 10) */
static const double air_log_re[] = { -1,
				     0,
				     1,
				     2,
				     3,
				     4,
				     5,
				     6 };
static const double air_log_drag_coeff[] = { 2.25,
					     1.35,
					     0.6,
					     0.0,
					     -0.35,
					     -0.45,
					     -0.33,
					     -0.9 };

/* Minimum time step for ODE solver (s) */
#define MIN_TIME_STEP 0.01

/* Maximum time step for ODE solver (s) */
#define MAX_TIME_STEP 0.10

/* Maximum distance of step for ODE solver (m) */
#define MAX_STEP_DISTANCE 0.20

/* Tolerance on error in Tux's position (m) */
#define MAX_POSITION_ERROR 0.005

/* Tolerance on error in Tux's velocity (m/s) */
#define MAX_VELOCITY_ERROR 0.05

/* To smooth out the terrain, the terrain normals are interpolated
   near the edges of the triangles.  This parameter controls how much
   smoothing is done, as well as the size of the triangle boundary
   over which smoothing occurs.  A value of 0 will result in no
   smoothing, while a value of 0.5 will smooth the normals over the
   entire surface of the triangle (though the smoothing will increase
   as the value approaches infinity).  (dimensionless) */
#define NORMAL_INTERPOLATION_LIMIT 0.05

/* The distance to move Tux up by so that he doesn't sit too low on the 
   ground (m) */
#define TUX_Y_CORRECTION_ON_STOMACH 0.33

/* The square of the distance that Tux must move before we recompute a 
   collision (m^2) */
#define COLLISION_TOLERANCE 0.04

/* Duraction of paddling motion (s) */
#define PADDLING_DURATION 0.40

/* Force applied against ground when paddling (N) */
#define MAX_PADDLING_FORCE 122.5

/* Ideal paddling friction coefficient */
#define IDEAL_PADDLING_FRIC_COEFF 0.35

/* G force applied for jump with charge of 0 */
#define BASE_JUMP_G_FORCE 1.5

/* Maximum G force applied during jump */
#define MAX_JUMP_G_FORCE 3

/* Magnitude of force before damage is incurred (N) */
#define DAMAGE_RESISTANCE ( 4.0 * TUX_MASS * EARTH_GRAV )

/* Damage scaling factor (health/(N*s)) */
#define DAMAGE_SCALE 9e-8

/* Amount to scale tree force by (so that it does more damage than regular
   collisions) (dimensionless) */
#define COLLISION_BASE_DAMAGE 0.02

/* Damage incurred by paddling exertion (health/s) */
#define PADDLING_DAMAGE 0.02


/* Wind velocity */
static pp::Vec3d wind_vel(250.0/3.6, 0, 0);
static double wind_scale = 0.5;

/*
 * Static variables
 */

/* Friction coefficients (dimensionless) */
static double  fricCoeff[4] = { 0.35, /* ice */
                                  0.7,  /* rock */
                                  0.45,  /* snow */
								  -0.50  /* ramp */
                                };


/* Current time step in ODE solver */
static double ode_time_step = -1;

class Index2d{
public:
	Index2d(){};
	Index2d(const int i, const int j)
	{
		this->i=i;
		this->j=j;
	}	
	int i;   
    int j;
};	

void
set_wind_velocity(pp::Vec3d velocity, float scale)
{
	wind_vel = velocity;
	wind_scale = scale;							
}

void increment_turn_fact( Player& plyr, double amt )
{
    plyr.control.turn_fact += amt;
    plyr.control.turn_fact = MIN( 1.0, MAX( plyr.control.turn_fact, -1.0 ) );
}

double get_min_tux_speed()
{
    return MIN_TUX_SPEED;   
}

void set_friction_coeff( const float fric[3] ) 
{
    fricCoeff[0] = fric[0];
    fricCoeff[1] = fric[1];
    fricCoeff[2] = fric[2];
	fricCoeff[3] = fric[3];
} 

float get_min_y_coord() 
{
    float courseWidth, courseLength;
    float angle;
    float minY;

    get_course_dimensions( &courseWidth, &courseLength );
    angle = get_course_angle();

    minY = -courseLength * tan( ANGLES_TO_RADIANS( angle ) );
    return minY;
} 

void get_indices_for_point( double x, double z, 
			    int *x0, int *y0, int *x1, int *y1 )
{
    float courseWidth, courseLength;
    float xidx, yidx;
    int nx, ny;

    get_course_dimensions( &courseWidth, &courseLength );
    get_course_divisions( &nx, &ny );
    
    xidx = x / courseWidth * ( (float) nx - 1. );
    yidx = -z / courseLength * ( (float) ny - 1. );

    if (xidx < 0) {
        xidx = 0;
    } else if ( xidx > nx-1 ) {
        xidx = nx-1;
    } 

    if (yidx < 0) {
        yidx = 0;
    } else if ( yidx > ny-1 ) {
        yidx = ny-1;
    } 

    /* I found that ceil(3) was quite slow on at least some architectures, 
       so I've replace it with an approximation */
    *x0 = (int) (xidx);              /* floor(xidx) */
    *x1 = (int) ( xidx + 0.9999 );   /* ceil(xidx) */
    *y0 = (int) (yidx);              /* floor(yidx) */
    *y1 = (int) ( yidx + 0.9999 );   /* ceil(yidx) */

    if ( *x0 == *x1 ) {
	if ( *x1 < nx - 1 ) 
	    (*x1)++;
	else 
	    (*x0)--;
    } 

    if ( *y0 == *y1 ) {
	if ( *y1 < ny - 1 )
	    (*y1)++;
	else 
	    (*y0)--;
    } 

    check_assertion( *x0 >= 0, "invalid x0 index" );
    check_assertion( *x0 < nx, "invalid x0 index" );
    check_assertion( *x1 >= 0, "invalid x1 index" );
    check_assertion( *x1 < nx, "invalid x1 index" );
    check_assertion( *y0 >= 0, "invalid y0 index" );
    check_assertion( *y0 < ny, "invalid y0 index" );
    check_assertion( *y1 >= 0, "invalid y1 index" );
    check_assertion( *y1 < ny, "invalid y1 index" );
}


void find_barycentric_coords( float x, float z, 
			      Index2d *idx0, 
			      Index2d *idx1, 
			      Index2d *idx2,
			      float *u, float *v )
{
    float xidx, yidx;
    int nx, ny;
    int x0, x1, y0, y1;
    float dx, ex, dz, ez, qx, qz, invdet; // to calc. barycentric coords 
    float courseWidth, courseLength;
    float *elevation;

    elevation = get_course_elev_data();
    get_course_dimensions( &courseWidth, &courseLength );
    get_course_divisions( &nx, &ny );

    get_indices_for_point( x, z, &x0, &y0, &x1, &y1 );
    
    xidx = x / courseWidth * ( (float) nx - 1. );
    yidx = -z / courseLength * ( (float) ny - 1. );


    /* The terrain is meshed as follows:
	      ...
	   +-+-+-+-+            x<---+
	   |\|/|\|/|                 |
        ...+-+-+-+-+...              V
	   |/|\|/|\|                 y
	   +-+-+-+-+
	      ...
       
       So there are two types of squares: those like this (x0+y0 is even):

       +-+
       |/|
       +-+

       and those like this (x0+y0 is odd).
       
       +-+
       |\|
       +-+
    */


    if ( (x0 + y0) % 2 == 0 ) {
	if ( yidx - y0 < xidx - x0 ) {
	    *idx0 = Index2d(x0, y0); 
	    *idx1 = Index2d(x1, y0); 
	    *idx2 = Index2d(x1, y1); 
	} else {
	    *idx0 = Index2d(x1, y1); 
	    *idx1 = Index2d(x0, y1); 
	    *idx2 = Index2d(x0, y0); 
	} 
    } else {
	// x0 + y0 is odd 
	if ( yidx - y0 + xidx - x0 < 1 ) {
	    *idx0 = Index2d(x0, y0); 
	    *idx1 = Index2d(x1, y0); 
	    *idx2 = Index2d(x0, y1); 
	} else {
	    *idx0 = Index2d(x1, y1); 
	    *idx1 = Index2d(x0, y1); 
	    *idx2 = Index2d(x1, y0); 
	} 
    }

    dx = idx0->i - idx2->i;
    dz = idx0->j - idx2->j;
    ex = idx1->i - idx2->i;
    ez = idx1->j - idx2->j;
    qx = xidx - idx2->i;
    qz = yidx - idx2->j;

    invdet = 1./(dx * ez - dz * ex);
    *u = (qx * ez - qz * ex) * invdet;
    *v = (qz * dx - qx * dz) * invdet;
}


pp::Vec3d find_course_normal( const float x, const float z )
{
    pp::Vec3d *course_nmls;
    pp::Vec3d tri_nml;
    float xidx, yidx;
    int nx, ny;
    int x0, x1, y0, y1;
    pp::Vec3d p0, p1, p2;
    Index2d idx0, idx1, idx2;
    pp::Vec3d n0, n1, n2;
    float course_width, course_length;
    float u, v;
    float min_bary, interp_factor;
    pp::Vec3d smooth_nml;
    pp::Vec3d interp_nml;
    float *elevation;

    elevation = get_course_elev_data();
    get_course_dimensions( &course_width, &course_length );
    get_course_divisions( &nx, &ny );
    course_nmls = get_course_normals();

    get_indices_for_point( x, z, &x0, &y0, &x1, &y1 );
    
    xidx = x / course_width * ( (float) nx - 1. );
    yidx = -z / course_length * ( (float) ny - 1. );

    find_barycentric_coords( x, z, &idx0, &idx1, &idx2, &u, &v );

    n0 = course_nmls[ idx0.i + nx * idx0.j ];
    n1 = course_nmls[ idx1.i + nx * idx1.j ];
    n2 = course_nmls[ idx2.i + nx * idx2.j ];

    p0 = COURSE_VERTEX( idx0.i, idx0.j );
    p1 = COURSE_VERTEX( idx1.i, idx1.j );
    p2 = COURSE_VERTEX( idx2.i, idx2.j );
    
    smooth_nml = (u*n0)+(v*n1+( 1.-u-v)*n2);

    tri_nml = (p1-p0)^(p2-p0);
    tri_nml.normalize();

    min_bary = MIN( u, MIN( v, 1. - u - v ) );
    interp_factor = MIN( min_bary / NORMAL_INTERPOLATION_LIMIT, 1.0 );

    interp_nml = (interp_factor*tri_nml)+
	((1.-interp_factor)*smooth_nml);
    interp_nml.normalize();

    return interp_nml;
}

float find_y_coord( float x, float z )
{
    float ycoord;
    pp::Vec3d p0, p1, p2;
    Index2d idx0, idx1, idx2;
    float u, v;
    int nx, ny;
    float *elevation;
    float course_width, course_length;

    /* cache last request */
    static float last_x, last_z, last_y;
    static bool cache_full = false;

    if ( cache_full && last_x == x && last_z == z ) {
	return last_y;
    }

    get_course_divisions( &nx, &ny );
    get_course_dimensions( &course_width, &course_length );
    elevation = get_course_elev_data();

    find_barycentric_coords( x, z, &idx0, &idx1, &idx2, &u, &v );

    p0 = COURSE_VERTEX( idx0.i, idx0.j );
    p1 = COURSE_VERTEX( idx1.i, idx1.j );
    p2 = COURSE_VERTEX( idx2.i, idx2.j );

    ycoord = u * p0.y + v * p1.y + ( 1. - u - v ) * p2.y;

    last_x = x;
    last_z = z;
    last_y = ycoord;
    cache_full = true;

    return ycoord;
} 

void get_surface_type( float x, float z, float weights[] )
{
    int *terrain;
    float courseWidth, courseLength;
    int nx, ny;
    Index2d idx0, idx1, idx2;
    float u, v;
    unsigned int i;

    find_barycentric_coords( x, z, &idx0, &idx1, &idx2, &u, &v );

    terrain = get_course_terrain_data();
    get_course_dimensions( &courseWidth, &courseLength );
    get_course_divisions( &nx, &ny );

    for (i=0; i<num_terrains; i++) {
	weights[i] = 0;
	if ( terrain[ idx0.i + nx*idx0.j ] == int(i) ) {
	    weights[i] += u;
	}
	if ( terrain[ idx1.i + nx*idx1.j ] == int(i) ) {
	    weights[i] += v;
	}
	if ( terrain[ idx2.i + nx*idx2.j ] == int(i) ) {
	    weights[i] += 1.0 - u - v;
	}
    }
} 

pp::Plane get_local_course_plane( pp::Vec3d pt )
{
    pp::Plane plane;

    pt.y = find_y_coord( pt.x, pt.z );

    plane.nml = find_course_normal( pt.x, pt.z );
    plane.d = -( plane.nml.x * pt.x + 
		 plane.nml.y * pt.y +
		 plane.nml.z * pt.z );

    return plane;
}

static void update_paddling( Player& plyr )
{
    if ( plyr.control.is_paddling ) {
		if ( gameMgr->time - plyr.control.paddle_time >= PADDLING_DURATION ) {
		    print_debug( DEBUG_CONTROL, "paddling off" );
		    plyr.control.is_paddling = false;
		}
    }
}

void set_tux_pos( Player& plyr, pp::Vec3d new_pos )
{
    char *tuxRoot;
    float playWidth, playLength;
    float courseWidth, courseLength;
    float boundaryWidth;
    float disp_y;

    get_play_dimensions( &playWidth, &playLength );
    get_course_dimensions( &courseWidth, &courseLength );
    boundaryWidth = ( courseWidth - playWidth ) / 2; 


    if ( new_pos.x < boundaryWidth ) {
        new_pos.x = boundaryWidth;
    } else if ( new_pos.x > courseWidth - boundaryWidth ) {
        new_pos.x = courseWidth - boundaryWidth;
    } 

    if ( new_pos.z > 0 ) {
        new_pos.z = 0;
    } else if ( -new_pos.z >= playLength ) {
        new_pos.z = -playLength;
        set_game_mode( GAME_OVER );
    } 

    plyr.pos = new_pos;

    disp_y = new_pos.y + TUX_Y_CORRECTION_ON_STOMACH; 

    tuxRoot = ModelHndl->get_tux_root_node();
    reset_scene_node( tuxRoot );
    translate_scene_node( tuxRoot, 
			  pp::Vec3d( new_pos.x, disp_y, new_pos.z ) );
} 

bool check_tree_collisions( Player& plyr, pp::Vec3d pos, 
			      pp::Vec3d *tree_loc, float *tree_diam )
{
    if(getparam_disable_collision_detection()){
		return false;
	}	
	
	Tree *trees;
    int num_trees, i;
    float diam = 0.0; 
    float height;
	pp::Polyhedron *ph, ph2;
    pp::Matrix mat;
    pp::Vec3d loc;
    bool hit = false;
    pp::Vec3d distvec;
    char *tux_root;
    float squared_dist;
    int tree_type;
    
    /* These variables are used to cache collision detection results */
    static bool last_collision = false;
    static pp::Vec3d last_collision_tree_loc( -999, -999, -999 );
    static double last_collision_tree_diam = 0;
    static pp::Vec3d last_collision_pos( -999, -999, -999 );

    /* If we haven't moved very much since the last call, we re-use
       the results of the last call (significant speed-up) */
	   
	  
    pp::Vec3d dist_vec = pos - last_collision_pos;
    if ( dist_vec.length2() < COLLISION_TOLERANCE ) {
	if ( last_collision ) {
	    if ( tree_loc != NULL ) {
		*tree_loc = last_collision_tree_loc;
	    }
	    if ( tree_diam != NULL ) {
		*tree_diam = last_collision_tree_diam;
	    }
	    return true;
	} else {
	    return false;
	}
    }

    trees = get_tree_locs();
    num_trees = get_num_trees();
    tree_type = trees[0].type;
    ph = get_tree_polyhedron(tree_type);

    for (i=0; i<num_trees; i++) {
        diam = trees[i].diam;
        height = trees[i].height;
        loc = trees[i].ray.pt;

        distvec = pp::Vec3d( loc.x - pos.x, 0.0, loc.z - pos.z );

	/* check distance from tree; .6 is the radius of a bounding
           sphere around tux (approximate) */
	squared_dist = ( diam/2. + 0.6 );
	squared_dist *= squared_dist;
        if ( distvec.length2() > squared_dist ) {
            continue;
        } 

	/* have to look at polyhedron - switch to correct one if necessary */
	if ( tree_type != trees[i].type )  {
	    tree_type = trees[i].type;
	    ph = get_tree_polyhedron(tree_type);
		
		
		
	}
        ph2 = copy_polyhedron( *ph );
	
        //mat.makeScaling( diam, height, diam );
		mat.makeScaling( diam, diam, diam );
	
        trans_polyhedron( mat, ph2 );
        mat.makeTranslation( loc.x, loc.y, loc.z );
        trans_polyhedron( mat, ph2 );

	tux_root = ModelHndl->get_tux_root_node();
	reset_scene_node( tux_root );
	translate_scene_node( tux_root, 
			      pp::Vec3d( pos.x, pos.y, pos.z ) );
        hit = collide( tux_root, ph2 );
		
        free_polyhedron( ph2 );

        if ( hit == true ) {
	    if ( tree_loc != NULL ) {
		*tree_loc = loc;
	    }
	    if ( tree_diam != NULL ) {
		*tree_diam = diam;
	    }


            break;
        } 
    } 

    last_collision_tree_loc = loc;
    last_collision_tree_diam = diam;
    last_collision_pos = pos;

    if ( hit ) {
	last_collision = true;

	/* Record collision in player data so that health can be adjusted */
	plyr.collision = true;
    } else {
	last_collision = false;
    }

    return hit;
} 





void check_item_collection( Player& plyr, pp::Vec3d pos )
{
    Item *items;
    int num_items, i;
    float diam = 0.0; 
    float height;
    pp::Vec3d loc;
    pp::Vec3d distvec;
    float squared_dist;
    int item_type;

    /* These variables are used to cache collision detection results */
    static pp::Vec3d last_collision_pos( -999, -999, -999 );

    /* If we haven't moved very much since the last call, we re-use
       the results of the last call (significant speed-up) */
    pp::Vec3d dist_vec = pos - last_collision_pos;
    if ( dist_vec.length2() < COLLISION_TOLERANCE ) {
		return ;
    }

    items = get_item_locs();
    num_items = get_num_items();
    item_type = items[0].type;

    for (i=0; i<num_items; i++) {

		/*
		if ( items[i].isCollectable()==false) {
		    continue;
		}
		*/
		if ( items[i].getType() == Item::UNCOLLECTABLE ||
			 items[i].isCollected() )
		{
		    continue;
		}

        diam = items[i].diam;
        height = items[i].height;
        loc = items[i].ray.pt;

        distvec = pp::Vec3d( loc.x - pos.x, 0.0, loc.z - pos.z );

	/* check distance from tree; .6 is the radius of a bounding
           sphere around tux (approximate) */
	squared_dist = ( diam/2.0 + 0.6 );
	squared_dist *= squared_dist;
        if ( distvec.length2() > squared_dist ) {
            continue;
        } 

	if ( (pos.y - 0.6 >= loc.y && pos.y - 0.6 <= loc.y + height) ||
	     (pos.y + 0.6 >= loc.y && pos.y + 0.6 <= loc.y + height) ||
	     (pos.y - 0.6 <= loc.y && pos.y + 0.6 >= loc.y + height) ) {
	    /* close enough to hitting the flag */
	    /* play a noise? */
	    items[i].setCollected();
		items[i].setDrawable(false);	 
			 
		if(items[i].getType()==Item::HERRING){
			plyr.herring += items[i].getScore();
			if (plyr.herring <0) plyr.herring=0;
		}else{
			plyr.incLives();			
		}

	    play_sound( "item_collect", 0 );
	}

    } 

} 

float get_compression_depth( const int terrain ) 
{
	return terrain_texture[terrain].compression;
}

/*
 * Check for tree collisions and adjust position and velocity appropriately.
 */
static void adjust_for_tree_collision( Player& plyr, 
				       pp::Vec3d pos, pp::Vec3d *vel )
{
    pp::Vec3d treeNml;
    pp::Vec3d treeLoc;
    bool treeHit;
    float speed;
    float costheta;
    float tree_diam;

    treeHit = check_tree_collisions( plyr, pos, &treeLoc, &tree_diam );
    if (treeHit) {
	/*
	 * Calculate the normal vector to the tree; here we model the tree
	 * as a vertical cylinder.
	 */
        treeNml.x = pos.x - treeLoc.x;
        treeNml.y = 0;
        treeNml.z = pos.z - treeLoc.z; 
        treeNml.normalize();

	/* Reduce speed by a minimum of 30% */
        speed = vel->normalize();
        speed *= 0.7;

	/* 
	 * If Tux is moving into the tree, reduce the speed further, 
	 * and reflect the velocity vector off of the tree
	 */
        costheta = *vel*treeNml;
        if (costheta < 0 ) {
	    /* Reduce speed */
	    
            speed *= 1 + costheta;
            speed *= 1 + costheta; 

	    /* Do the reflection */
            *vel = ((-2. * ( *vel* treeNml ))*treeNml)+*vel;

	    vel->normalize();

        } 

	speed = MAX( speed, get_min_tux_speed() );
        *vel = speed*(*vel);
    } 
}

/*
 * Adjusts velocity so that his speed doesn't drop below the minimum 
 * speed
 */
double adjust_velocity( pp::Vec3d *vel, pp::Vec3d pos, 
			  pp::Plane surf_plane, float dist_from_surface )
{
    pp::Vec3d surf_nml;
    float speed;

    surf_nml = surf_plane.nml;

    speed = vel->normalize();

    if ( speed < EPS )
    {
	if ( fabs( surf_nml.x ) + fabs( surf_nml.z ) > EPS ) {
	    *vel = projectIntoPlane( 
		surf_nml, pp::Vec3d( 0.0, -1.0, 0.0 ) );
	    vel->normalize();
	} else {
	    *vel = pp::Vec3d( 0.0, 0.0, -1.0 );
	}
    }

    speed = MAX( get_min_tux_speed(), speed );

    *vel = speed*(*vel);
    return speed;
}

void adjust_position( pp::Vec3d *pos, pp::Plane surf_plane, 
		      float dist_from_surface )
{

    if ( dist_from_surface < -MAX_SURFACE_PENETRATION )
    {
	*pos = ( *pos+ (-MAX_SURFACE_PENETRATION - dist_from_surface)* surf_plane.nml);
    }
    return;
}

static pp::Vec3d adjust_tux_zvec_for_roll( Player& plyr, 
					  pp::Vec3d vel, pp::Vec3d zvec )
{
    pp::Matrix rot_mat; 

    vel = projectIntoPlane( zvec, vel );

    vel.normalize();

    if ( plyr.control.is_braking ) {
	rot_mat.makeRotationAboutVector( vel, 
					   plyr.control.turn_fact *
					   BRAKING_ROLL_ANGLE );
    } else {
	rot_mat.makeRotationAboutVector( vel, 
					   plyr.control.turn_fact *
					   MAX_ROLL_ANGLE );
    }

    return rot_mat.transformVector(zvec);
}

static pp::Vec3d adjust_surf_nml_for_roll( Player& plyr, 
					  pp::Vec3d vel, 
					  float fric_coeff,
					  pp::Vec3d nml )
{
    pp::Matrix rot_mat; 
    float angle;
    float speed;
    float roll_angle;

    speed = vel.normalize();

    vel = projectIntoPlane( nml, vel );

    vel.normalize();
    if ( plyr.control.is_braking ) {
	roll_angle = BRAKING_ROLL_ANGLE;
    } else {
	roll_angle = MAX_ROLL_ANGLE;
    }

	angle = plyr.control.turn_fact *
	roll_angle *
	MIN( 1.0, MAX(0.0, fric_coeff)/IDEAL_ROLL_FRIC_COEFF )
	* MIN(1.0, MAX(0.0,speed-MIN_TUX_SPEED)/
	      (IDEAL_ROLL_SPEED-MIN_TUX_SPEED));
    
    rot_mat.makeRotationAboutVector( vel, angle );

    return rot_mat.transformVector(nml);
}


void adjust_orientation( Player& plyr, float dtime, 
			 pp::Vec3d pos, pp::Vec3d vel,
			 float dist_from_surface, pp::Vec3d surf_nml )
{
    pp::Vec3d new_x, new_y, new_z; 
    pp::Matrix inv_cob_mat;
    pp::Matrix rot_mat;
    pp::Quat new_orient;
    char* tux_root;
    float time_constant;
    static pp::Vec3d minus_z_vec(0., 0., -1.);
    static pp::Vec3d y_vec(0., 1., 0.);

    if ( dist_from_surface > 0 ) {
	new_y = 1.*vel;
	new_y.normalize();
	new_z = projectIntoPlane( new_y, pp::Vec3d(0., -1., 0.) );
	new_z.normalize();
	new_z = adjust_tux_zvec_for_roll( plyr, vel, new_z );
    } else { 
	new_z = -1.*surf_nml;
	new_z = adjust_tux_zvec_for_roll( plyr, vel, new_z );
	new_y = projectIntoPlane( surf_nml,1.*vel);
	new_y.normalize();
    }

    new_x = new_y^new_z;

	{
		pp::Matrix cob_mat;    
		pp::Matrix::makeChangeOfBasisMatrix( cob_mat, inv_cob_mat, new_x, new_y, new_z );
	    new_orient = pp::Quat(cob_mat);
	}
    if ( !plyr.orientation_initialized ) {
	plyr.orientation_initialized = true;
	plyr.orientation = new_orient;
    }

    time_constant = dist_from_surface > 0
	? TUX_ORIENTATION_AIRBORNE_TIME_CONSTANT
	: TUX_ORIENTATION_TIME_CONSTANT;

    plyr.orientation = pp::Quat::interpolate( 
	plyr.orientation, new_orient, 
	MIN( dtime / time_constant, 1.0 ) );

    plyr.plane_nml = plyr.orientation.rotate( minus_z_vec );
    plyr.direction = plyr.orientation.rotate( y_vec );

    pp::Matrix cob_mat( plyr.orientation );


    /* Trick rotations */
    new_y = pp::Vec3d( cob_mat.data[1][0], cob_mat.data[1][1], cob_mat.data[1][2] ); 
    rot_mat.makeRotationAboutVector( new_y, 
				       ( plyr.control.barrel_roll_factor * 360 ) );
    cob_mat=rot_mat*cob_mat;
    new_x = pp::Vec3d( cob_mat.data[0][0], cob_mat.data[0][1], cob_mat.data[0][2] ); 
    rot_mat.makeRotationAboutVector( new_x, 
				       plyr.control.flip_factor * 360 );
    cob_mat=rot_mat*cob_mat;



    inv_cob_mat.transpose(cob_mat);

    tux_root = ModelHndl->get_tux_root_node();
    transform_scene_node( tux_root, cob_mat, inv_cob_mat ); 
}

float adjust_particle_count( float particles ) 
{
    if ( particles < 1 ) {
	if ( ( (float) rand() ) / RAND_MAX < particles ) {
	    return 1.0;
	} else {
	    return 0.0;
	}
    } else {
	return particles;
    }
}

void generate_particles( Player& plyr, float dtime, 
			 pp::Vec3d pos, float speed ) 
{
    pp::Vec3d left_part_pt, right_part_pt;
    float brake_particles;
    float turn_particles;
    float roll_particles;
    float surf_weights[NUM_TERRAIN_TYPES]; 
    float surf_y;
    float left_particles, right_particles;
    pp::Vec3d left_part_vel, right_part_vel;
    pp::Matrix rot_mat;
    pp::Vec3d xvec;
	bool particles_type;
	GLuint particle_binding = 0;
	
	unsigned int i;
	
    get_surface_type( pos.x, pos.z, surf_weights );
    surf_y = find_y_coord( pos.x, pos.z );

	
	particles_type=false;
	for (i=0;i<num_terrains;i++){
		if (terrain_texture[i].partbind != 0){	
			if (surf_weights[i] > 0.5) {
				particles_type=true;
				particle_binding = terrain_texture[i].partbind;
    		}
		}
	}
	
	
	
	
    if ( particles_type== true && pos.y < surf_y ) {
	xvec =plyr.direction^plyr.plane_nml;

        right_part_pt = left_part_pt = pos;

	right_part_pt = right_part_pt + ((TUX_WIDTH/2.0)*xvec );

	left_part_pt = left_part_pt + ((-TUX_WIDTH/2.0)* xvec);

        right_part_pt.y = left_part_pt.y  = surf_y;

	brake_particles = dtime *
	    BRAKE_PARTICLES * ( plyr.control.is_braking ? 1.0 : 0.0 )
	    * MIN( speed / PARTICLE_SPEED_FACTOR, 1.0 );
	turn_particles = dtime * MAX_TURN_PARTICLES 
	    * MIN( speed / PARTICLE_SPEED_FACTOR, 1.0 );
	roll_particles = dtime * MAX_ROLL_PARTICLES 
	    * MIN( speed / PARTICLE_SPEED_FACTOR, 1.0 );

	left_particles = turn_particles * 
	    fabs( MIN(plyr.control.turn_fact, 0.) ) + 
	    brake_particles +
	    roll_particles * fabs( MIN(plyr.control.turn_animation, 0.) );

	right_particles = turn_particles * 
	    fabs( MAX(plyr.control.turn_fact, 0.) ) + 
	    brake_particles +
	    roll_particles * fabs( MAX(plyr.control.turn_animation, 0.) );

	left_particles = adjust_particle_count( left_particles );
	right_particles = adjust_particle_count( right_particles );

	/* Create particle velocitites */
	rot_mat.makeRotationAboutVector( plyr.direction,
	    MAX( -MAX_PARTICLE_ANGLE, 
		 -MAX_PARTICLE_ANGLE * speed / MAX_PARTICLE_ANGLE_SPEED ) );
	left_part_vel = rot_mat.transformVector(plyr.plane_nml);
	left_part_vel = MIN( MAX_PARTICLE_SPEED, 
					   speed * PARTICLE_SPEED_MULTIPLIER )*
				      left_part_vel;
	rot_mat.makeRotationAboutVector( plyr.direction,
	    MIN( MAX_PARTICLE_ANGLE, 
		 MAX_PARTICLE_ANGLE * speed / MAX_PARTICLE_ANGLE_SPEED ) );
	right_part_vel = rot_mat.transformVector( plyr.plane_nml );
	right_part_vel = MIN( MAX_PARTICLE_SPEED,
					    speed * PARTICLE_SPEED_MULTIPLIER )*
				       right_part_vel;

        create_new_particles( left_part_pt, left_part_vel, 
			      (int)left_particles, particle_binding );
        create_new_particles( right_part_pt, right_part_vel, 
			      (int)right_particles, particle_binding );
    } 
}


/*
 * Calculate the magnitude of force due to air resistance (wind)
 */
pp::Vec3d calc_wind_force( pp::Vec3d player_vel )
{
    pp::Vec3d total_vel;
    float wind_speed;
    float re;         /* Reynolds number */
    float df_mag;     /* magnitude of drag force */
    float drag_coeff; /* drag coefficient */
    int table_size;      /* size of drag coeff table */

    static float last_time_called = -1;

    total_vel = -1*player_vel;
    
    if ( gameMgr->getCurrentRace().windy ) {
	/* adjust wind_scale with a random walk */
	if ( last_time_called != gameMgr->time ) {
	    wind_scale = wind_scale + 
		(rand()/(double)RAND_MAX-0.50) * 0.15;
	    wind_scale = MIN( 1.0, MAX( 0.0, wind_scale ) );
	}

	total_vel = total_vel+(wind_scale*wind_vel);
    }

    wind_speed = total_vel.normalize();

    re = AIR_DENSITY * wind_speed * TUX_DIAMETER / AIR_VISCOSITY;

    table_size = sizeof(air_log_drag_coeff) / sizeof(air_log_drag_coeff[0]);

    drag_coeff = pow( 10.0, 
		      lin_interp( air_log_re, air_log_drag_coeff, 
				  log10(re), table_size ) );

    df_mag = 0.5 * drag_coeff * AIR_DENSITY * ( wind_speed * wind_speed )
	* ( M_PI * ( TUX_DIAMETER * TUX_DIAMETER ) * 0.25 );

    check_assertion( df_mag > 0, "Negative wind force" );

    last_time_called = gameMgr->time;

    return df_mag*total_vel;
}

static pp::Vec3d calc_spring_force( float compression, pp::Vec3d vel, 
				   pp::Vec3d surf_nml, pp::Vec3d *unclamped_f )
{
    float spring_vel; /* velocity perp. to surface (for damping) */
    float spring_f_mag; /* magnitude of force */

   check_assertion( compression >= 0, 
		    "spring can't have negative compression" );
    
    spring_vel =vel*surf_nml;

    /* Stage 1 */
    spring_f_mag = 
	MIN( compression, TUX_GLUTE_STAGE_1_COMPRESSIBILITY ) 
	* TUX_GLUTE_STAGE_1_SPRING_COEFF;

    /* Stage 2 */
    spring_f_mag += 
	MAX( 0, MIN( compression - TUX_GLUTE_STAGE_1_COMPRESSIBILITY, 
		     TUX_GLUTE_STAGE_2_COMPRESSIBILITY ) )
	* TUX_GLUTE_STAGE_2_SPRING_COEFF;

    /* Stage 3 */
    spring_f_mag += 
	MAX( 0., compression - TUX_GLUTE_STAGE_2_COMPRESSIBILITY - 
	     TUX_GLUTE_STAGE_1_COMPRESSIBILITY ) 
	* TUX_GLUTE_STAGE_3_SPRING_COEFF;

    /* Damping */
    spring_f_mag -= spring_vel * ( 
	compression <= TUX_GLUTE_STAGE_1_COMPRESSIBILITY 
	? TUX_GLUTE_STAGE_1_SPRING_COEFF : 
	( compression <= TUX_GLUTE_STAGE_2_COMPRESSIBILITY 
	  ? TUX_GLUTE_STAGE_2_DAMPING_COEFF
	  : TUX_GLUTE_STAGE_3_DAMPING_COEFF ) );

    /* Clamp to >= 0.0 */
    spring_f_mag = MAX( spring_f_mag, 0.0 );

    if ( unclamped_f != NULL ) {
	*unclamped_f = spring_f_mag*surf_nml;
    }

    /* Clamp to <= TUX_GLUTE_MAX_SPRING_FORCE */
    spring_f_mag = MIN( spring_f_mag, TUX_GLUTE_MAX_SPRING_FORCE );

    return spring_f_mag*surf_nml;
}


static pp::Vec3d calc_net_force( Player& plyr, pp::Vec3d pos, 
				pp::Vec3d vel )
{
    pp::Vec3d nml_f;      /* normal force */
    pp::Vec3d unclamped_nml_f; /* unclamped normal force (for damage calc) */
    pp::Vec3d fric_f;     /* frictional force */
    float fric_f_mag; /* frictional force magnitude */
    pp::Vec3d fric_dir;   /* direction of frictional force */
    pp::Vec3d grav_f;     /* gravitational force */
    pp::Vec3d air_f;      /* air resistance force */
    pp::Vec3d brake_f;    /* braking force */
    pp::Vec3d paddling_f; /* paddling force */
    pp::Vec3d net_force;  /* the net force (sum of all other forces) */
    float comp_depth; /* depth to which the terrain can be compressed */
    float speed;      /* speed (m/s) */
    pp::Vec3d orig_surf_nml; /* normal to terrain at current position */
    pp::Vec3d surf_nml;   /* normal to terrain w/ roll effect */
    float glute_compression; /* amt that Tux's tush has been compressed */
    float steer_angle; /* Angle to rotate fricitonal force for turning */
    pp::Matrix fric_rot_mat; /* Matrix to rotate frictional force */
    pp::Vec3d jump_f;
    pp::Plane surf_plane;
    float dist_from_surface;
    float surf_weights[NUM_TERRAIN_TYPES];
    float surf_fric_coeff;
    unsigned int i;

    get_surface_type( pos.x, pos.z, surf_weights );
    surf_plane = get_local_course_plane( pos );
    orig_surf_nml = surf_plane.nml;

    surf_fric_coeff = 0;
    for (i=0; i<num_terrains; i++) {
	/*
	surf_fric_coeff += surf_weights[i] * fricCoeff[terrain_texture[i].type];
	*/
	surf_fric_coeff += surf_weights[i] * terrain_texture[i].friction;
    }
    surf_nml = adjust_surf_nml_for_roll( plyr, vel, surf_fric_coeff,
					 orig_surf_nml );
    
    comp_depth = 0;
    for (i=0; i<num_terrains; i++) {
	comp_depth += surf_weights[i] * get_compression_depth(i);
    }


    grav_f = pp::Vec3d( 0, -EARTH_GRAV * TUX_MASS, 0 );

    dist_from_surface = surf_plane.distance( pos );

    if ( dist_from_surface <= 0 ) {
	plyr.airborne = false;
    } else {
	plyr.airborne = true;
    }

    /*
     * Calculate normal force
     */
    nml_f = pp::Vec3d( 0., 0., 0. );
    if ( dist_from_surface <= -comp_depth ) {
	/*
	 * Tux ended up below the surface.
	 * Calculate the spring force exterted by his rear end. :-)
	 */
	glute_compression = -dist_from_surface - comp_depth;
	check_assertion( glute_compression >= 0, 
			 "unexpected negative compression" );

	nml_f = calc_spring_force( glute_compression, vel, surf_nml,
				   &unclamped_nml_f );
    }

    /* Check if player is trying to jump */
    if ( plyr.control.begin_jump == true ) {
	plyr.control.begin_jump = false;
	if ( dist_from_surface <= 0 ) {
	    plyr.control.jumping = true;
	    plyr.control.jump_start_time = gameMgr->time;
	} else {
	    plyr.control.jumping = false;
	}
    }


    /* Apply jump force in up direction for JUMP_FORCE_DURATION */
    if ( ( plyr.control.jumping ) &&
	 ( gameMgr->time - plyr.control.jump_start_time < 
	   JUMP_FORCE_DURATION ) ) 
    {
	jump_f = pp::Vec3d( 
	    0, 
	    BASE_JUMP_G_FORCE * TUX_MASS * EARTH_GRAV + 
	    plyr.control.jump_amt * 
	    (MAX_JUMP_G_FORCE-BASE_JUMP_G_FORCE) * TUX_MASS * EARTH_GRAV, 
	    0 );

    } else {
	jump_f = pp::Vec3d( 0, 0, 0 );
	plyr.control.jumping = false;
    }

    /* Use the unclamped normal force for damage calculation purposes */
    plyr.normal_force = unclamped_nml_f;

    /* 
     * Calculate frictional force
     */
    fric_dir = vel;
    speed = fric_dir.normalize();
    fric_dir = -1.0*fric_dir;
    
    if ( dist_from_surface < 0 && speed > MIN_FRICTION_SPEED ) {
	pp::Vec3d tmp_nml_f = nml_f;

	fric_f_mag = tmp_nml_f.normalize() * surf_fric_coeff;

	fric_f_mag = MIN( MAX_FRICTIONAL_FORCE, fric_f_mag );

	fric_f = fric_f_mag*fric_dir;


	/*
	 * Adjust friction for steering
	 */
	steer_angle = plyr.control.turn_fact * MAX_TURN_ANGLE;

	if ( fabs( fric_f_mag * sin( ANGLES_TO_RADIANS( steer_angle ) ) ) >
	     MAX_TURN_PERPENDICULAR_FORCE ) 
	{
	    //check_assertion( fabs( plyr->control.turn_fact ) > 0,
		//	     "steer angle is non-zero when player is not "
		//	     "turning" );
	    steer_angle = RADIANS_TO_ANGLES( 
		asin( MAX_TURN_PERPENDICULAR_FORCE / fric_f_mag ) ) * 
		plyr.control.turn_fact / fabs( plyr.control.turn_fact );
	}
	fric_rot_mat.makeRotationAboutVector( orig_surf_nml, steer_angle );
	fric_f = fric_rot_mat.transformVector( fric_f );
	fric_f = (1.0 + MAX_TURN_PENALTY)*fric_f;


	/*
	 * Calculate braking force
	 */
	if ( speed > get_min_tux_speed() && plyr.control.is_braking ) {
	    brake_f = (surf_fric_coeff * BRAKE_FORCE)*fric_dir; 
	} else {
	    brake_f = pp::Vec3d( 0., 0., 0. );
	}
	
    } else {
	fric_f = brake_f = pp::Vec3d( 0., 0., 0. );
    }

    /*
     * Calculate air resistance
     */
    air_f = calc_wind_force( vel );


    /*
     * Calculate force from paddling
     */
    update_paddling( plyr );
    if ( plyr.control.is_paddling ) {
	if ( plyr.airborne ) {
	    paddling_f = pp::Vec3d( 0, 0, -TUX_MASS * EARTH_GRAV / 4.0 );
	    paddling_f = plyr.orientation.rotate( paddling_f );
	} else {
	    paddling_f = ( 
		-1 * MIN( MAX_PADDLING_FORCE,  
			  MAX_PADDLING_FORCE * 
			  ( MAX_PADDLING_SPEED - speed ) / MAX_PADDLING_SPEED * 
			  MIN(1.0, 
			      surf_fric_coeff/IDEAL_PADDLING_FRIC_COEFF)))*
		fric_dir;
	}
    } else {
	paddling_f = pp::Vec3d( 0., 0., 0. );
    }

    
    /*
     * Add all the forces 
     */
    net_force = jump_f+grav_f+nml_f+fric_f+air_f+brake_f+paddling_f;

    return net_force;
}

static float adjust_time_step_size( float h, pp::Vec3d vel )
{
    float speed;

    speed = vel.normalize();

    h = MAX( h, MIN_TIME_STEP );
    h = MIN( h, MAX_STEP_DISTANCE / speed );
    h = MIN( h, MAX_TIME_STEP );

    return h;
}

/*
 * Solves the system of ordinary differential equations governing Tux's 
 * movement.  Based on Matlab's ode23.m, (c) The MathWorks, Inc.
 */
void solve_ode_system( Player& plyr, float dtime ) 
{
    float t0, t, tfinal;
    ode_data_t *x, *y, *z, *vx, *vy, *vz; /* store estimates of derivs */
    ode_solver_t solver;
    float h;
    bool done = false;
    bool failed = false;
    pp::Vec3d new_pos;
    pp::Vec3d new_vel, tmp_vel;
    float speed;
    pp::Vec3d new_f;
    pp::Vec3d saved_pos;
    pp::Vec3d saved_vel, saved_f;
    float pos_err[3], vel_err[3], tot_pos_err, tot_vel_err;
    float err=0, tol=0;
    int i;

    /* Select a solver */
    switch ( getparam_ode_solver() ) {
    case EULER:
	solver = new_euler_solver();
	break;
    case ODE23:
	solver = new_ode23_solver();
	break;
    case ODE45:
	solver = new_ode45_solver();
	break;
    default:
	setparam_ode_solver( ODE23 );
	solver = new_ode23_solver();
    }

    /* Select an initial time step */
    h = ode_time_step;

    if ( h < 0 || solver.estimate_error == NULL ) {
	h = adjust_time_step_size( dtime, plyr.vel );
    }

    t0 = 0;
    tfinal = dtime;

    t = t0;

    /* Create variables to store derivative estimates & other data */
    x  = solver.new_ode_data();
    y  = solver.new_ode_data();
    z  = solver.new_ode_data();
    vx = solver.new_ode_data();
    vy = solver.new_ode_data();
    vz = solver.new_ode_data();

    /* initialize state */
    new_pos = plyr.pos;
    new_vel = plyr.vel;
    new_f   = plyr.net_force;

    /* loop until we've integrated from t0 to tfinal */
    while (!done) {

	if ( t >= tfinal ) {
	    print_warning( CRITICAL_WARNING, 
			   "t >= tfinal in solve_ode_system()" );
	    break;
	}

	/* extend h by up to 10% to reach tfinal */
	if ( 1.1 * h > tfinal - t ) {
	    h = tfinal-t;
	    check_assertion( h >= 0., "integrated past tfinal" );
	    done = true;
	}

        print_debug( DEBUG_ODE, "h: %g", h );

	saved_pos = new_pos;
	saved_vel = new_vel;
	saved_f = new_f;

	/* Loop until error is acceptable */
	failed = false;

	for (;;) {

	    /* Store initial conditions */
	    solver.init_ode_data( x, new_pos.x, h );
	    solver.init_ode_data( y, new_pos.y, h );
	    solver.init_ode_data( z, new_pos.z, h );
	    solver.init_ode_data( vx, new_vel.x, h );
	    solver.init_ode_data( vy, new_vel.y, h );
	    solver.init_ode_data( vz, new_vel.z, h );


	    /* We assume that the first estimate in all ODE solvers is equal 
	       to the final value of the last iteration */
	    solver.update_estimate( x, 0, new_vel.x );
	    solver.update_estimate( y, 0, new_vel.y );
	    solver.update_estimate( z, 0, new_vel.z );
	    solver.update_estimate( vx, 0, new_f.x / TUX_MASS );
	    solver.update_estimate( vy, 0, new_f.y / TUX_MASS );
	    solver.update_estimate( vz, 0, new_f.z / TUX_MASS );

	    /* Update remaining estimates */
	    for ( i=1; i < solver.num_estimates(); i++ ) {
		new_pos.x = solver.next_val( x, i );
		new_pos.y = solver.next_val( y, i );
		new_pos.z = solver.next_val( z, i );
		new_vel.x = solver.next_val( vx, i );
		new_vel.y = solver.next_val( vy, i );
		new_vel.z = solver.next_val( vz, i );

		solver.update_estimate( x, i, new_vel.x );
		solver.update_estimate( y, i, new_vel.y );
		solver.update_estimate( z, i, new_vel.z );

		new_f = calc_net_force( plyr, new_pos, new_vel );

		solver.update_estimate( vx, i, new_f.x / TUX_MASS );
		solver.update_estimate( vy, i, new_f.y / TUX_MASS );
		solver.update_estimate( vz, i, new_f.z / TUX_MASS );
	    }

	    /* Get final values */
	    new_pos.x = solver.final_estimate( x );
	    new_pos.y = solver.final_estimate( y );
	    new_pos.z = solver.final_estimate( z );

	    new_vel.x = solver.final_estimate( vx );
	    new_vel.y = solver.final_estimate( vy );
	    new_vel.z = solver.final_estimate( vz );

	    /* If the current solver can provide error estimates, update h
	       based on the error, and re-evaluate this step if the error is 
	       too large  */
	    if ( solver.estimate_error != NULL ) {

		/* Calculate the error */
		pos_err[0] = solver.estimate_error( x );
		pos_err[1] = solver.estimate_error( y );
		pos_err[2] = solver.estimate_error( z );

		vel_err[0] = solver.estimate_error( vx );
		vel_err[1] = solver.estimate_error( vy );
		vel_err[2] = solver.estimate_error( vz );

		tot_pos_err = 0.;
		tot_vel_err = 0.;
		for ( i=0; i<3; i++ ) {
		    pos_err[i] *= pos_err[i];
		    tot_pos_err += pos_err[i];
		    vel_err[i] *= vel_err[i];
		    tot_vel_err += vel_err[i];
		}
		tot_pos_err = sqrt( tot_pos_err );
		tot_vel_err = sqrt( tot_vel_err );

                print_debug( DEBUG_ODE, "pos_err: %g, vel_err: %g", 
			     tot_pos_err, tot_vel_err );

		if ( tot_pos_err / MAX_POSITION_ERROR >
		     tot_vel_err / MAX_VELOCITY_ERROR )
		{
		    err = tot_pos_err;
		    tol = MAX_POSITION_ERROR;
		} else {
		    err = tot_vel_err;
		    tol = MAX_VELOCITY_ERROR;
		}

		/* Update h based on error */
		if (  err > tol  && h > MIN_TIME_STEP + EPS  )
		{
		    done = false;
		    if ( !failed ) {
			failed = true;
			h *=  MAX( 0.5, 0.8 *
				   pow( tol/err, 
					float(solver.time_step_exponent()) ) );
		    } else {
			h *= 0.5;
		    }

		    h = adjust_time_step_size( h, saved_vel );

		    new_pos = saved_pos;
		    new_vel = saved_vel;
		    new_f = saved_f;
		
		} else {
		    /* Error is acceptable or h is at its minimum; stop */
		    break;
		}

	    } else {
		/* Current solver doesn't provide error estimates; stop */
		break;
	    }
	}

	/* Update time */
	t = t + h;

	tmp_vel = new_vel;
	speed = tmp_vel.normalize();

	/* only generate particles if we're drawing them */
	if ( getparam_draw_particles() ) {
	    generate_particles( plyr, h, new_pos, speed );
	}

	/* Calculate the final net force */
	new_f = calc_net_force( plyr, new_pos, new_vel );

	/* If no failures, compute a new h */
	if ( !failed && solver.estimate_error != NULL ) {
		double temp = 1.25 * pow((double)err / tol,(double)solver.time_step_exponent());
		if ( temp > 0.2 ) {
		h = h / temp;
	    } else {
		h = 5.0 * h;
	    }
	}

	/* Clamp h to constraints */
	h = adjust_time_step_size( h, new_vel );

	/* Important: to make trees "solid", we must manipulate the 
	   velocity here; if we don't and Tux is moving very quickly,
	   he can pass through trees */
	adjust_for_tree_collision( plyr, new_pos, &new_vel );

	/* Try to collect items here */
	check_item_collection( plyr, new_pos );

    }

    /* Save time step for next time */
    ode_time_step = h;

    plyr.vel = new_vel;
    plyr.pos = new_pos;
    plyr.net_force = new_f;

    free( x );
    free( y );
    free( z );
    free( vx );
    free( vy );
    free( vz );
}


/* 
 * Updates Tux's position taking into account gravity, friction, tree 
 * collisions, etc.  This is the main physics function.
 */
void update_player_pos( Player& plyr, float dtime )
{
    pp::Vec3d surf_nml;   /* normal vector of terrain */
    pp::Vec3d tmp_vel;
    float speed;
    float paddling_factor; 
    pp::Vec3d local_force;
    float flap_factor;
    pp::Plane surf_plane;
    float dist_from_surface;

    if ( dtime > 2. * EPS ) {
	solve_ode_system( plyr, dtime );
    }

    tmp_vel = plyr.vel;

    /*
     * Set position, orientation, generate particles
     */
    surf_plane = get_local_course_plane( plyr.pos );
    surf_nml = surf_plane.nml;
    dist_from_surface = surf_plane.distance( plyr.pos );
    adjust_velocity( &plyr.vel, plyr.pos, surf_plane, 
		     dist_from_surface );
    adjust_position( &plyr.pos, surf_plane, dist_from_surface );

    speed = tmp_vel.normalize();

    set_tux_pos( plyr, plyr.pos );
    adjust_orientation( plyr, dtime, plyr.pos, plyr.vel, 
			dist_from_surface, surf_nml );

    flap_factor = 0;

    if ( plyr.control.is_paddling ) {
	double factor;
	factor = (gameMgr->time - plyr.control.paddle_time) / PADDLING_DURATION;
	if ( plyr.airborne ) {
	    paddling_factor = 0;
	    flap_factor = factor;
	} else {
	    paddling_factor = factor;
	    flap_factor = 0;
	}
    } else {
	paddling_factor = 0.0;
    }

    /* calculate force in Tux's local coordinate system */
    local_force = plyr.orientation.conjugate().rotate(plyr.net_force);

    if (plyr.control.jumping) {
	flap_factor = (gameMgr->time - plyr.control.jump_start_time) / 
	    JUMP_FORCE_DURATION;
    } 

    ModelHndl->adjust_tux_joints( plyr.control.turn_animation, plyr.control.is_braking,
		       paddling_factor, speed, local_force, flap_factor );
}


void init_physical_simulation()
{
    pp::Vec3d nml;
    pp::Matrix rotMat;
    float ycoord;
    Player* plyr;
    pp::Vec3d init_vel;
    pp::Vec3d init_f;
    int i;

    for ( i=0; i<gameMgr->numPlayers; i++ ) {
		plyr = &players[i];

	ycoord = find_y_coord( plyr->pos.x, plyr->pos.z );
	nml = find_course_normal(plyr->pos.x, plyr->pos.z );
	rotMat.makeRotation( -90., 'x' );
	init_vel = rotMat.transformVector( nml );
	init_vel = INIT_TUX_SPEED*init_vel;
	init_f = pp::Vec3d( 0., 0., 0. );

	plyr->pos.y = ycoord;
	plyr->vel = init_vel;
	plyr->net_force = init_f;
	plyr->control.turn_fact = 0.0;
	plyr->control.turn_animation = 0.0;
	plyr->control.barrel_roll_factor = 0.0;
	plyr->control.flip_factor = 0.0;
	plyr->control.is_braking = false;
	plyr->orientation_initialized = false;
	plyr->plane_nml = nml;
	plyr->direction = init_vel;
	plyr->normal_force = pp::Vec3d(0,0,0);
	plyr->airborne = false;
	plyr->collision = false;
	plyr->control.jump_amt = 0;
	plyr->control.is_paddling = false;
	plyr->control.jumping = false;
	plyr->control.jump_charging = false;
	plyr->control.barrel_roll_left = false;
	plyr->control.barrel_roll_right = false;
	plyr->control.barrel_roll_factor = 0;
	plyr->control.front_flip = false;
	plyr->control.back_flip = false;
    }

    ode_time_step = -1;
}
