[ODE] Path Generation in ODE

[Niels Elburg]

Hello all,

I am trying to implement a small demo/game in ODE which uses an A*
algorithm for path generation.
The A* implementation works very well , it can find a route from origin O
to goal X and returns a vector with coordinates (x,y).

The ODE world is 2D , I am using Peter Serocka's 2D Joint
(http://www.mathematik.uni-bielefeld.de/~pserocka/ODE-Plane2D/)

Moving an object from node 0(x,y) to node 1(x,y) is not a problem,
but it is unable to follow the entire path. i.e. after a few nodes on
a straight line it must change direction (rotate) but the object
continuous in a straight line and collides with the borders of ODE's
world.

Do I need to slow down the object when it reaches a node , and rotate it
before applying new forces, so it could move to another ?

Below is a subset of the move_to routine I am using to move the object
in ODE's World.

Please advise,

Best regards,

 - Niels Elburg





static void		add_target_force // @@@ simulate motor :
force->pos
/******************************/
(
	dBody		&body,
	double		dir[2],	// normalized
	double		dist,
	double		crit_dist,
	double		max_speed,
	double		force_k
)
{
	const dReal	*curr_veloc = dBodyGetLinearVel (body.id ());
	double		curr_speed = VdotV2 (curr_veloc, dir);
	double		force;
	double		ortho_dir[2] = { -dir[1], dir[0] };


	// along dir:
	if (dist < crit_dist)
		max_speed *= dist / crit_dist;	 // decelerate
 	force = force_k * (max_speed - curr_speed);
	dBodyAddForce (body.id (), force * dir[0], force * dir[1], 0);

	// orthogonal to dir:
	curr_speed = VdotV2 (curr_veloc, ortho_dir);
	curr_speed -= (curr_speed / 10);
	force = force_k * (0 - curr_speed);
	dBodyAddForce (body.id (), force * ortho_dir[0], force *
ortho_dir[1], 0);
}



void			Creature::move_to
/*******************************/
(
	double		x,
	double		y
)
{
	dBody		&body = move_limb_->solid_->get_body ();
	const dReal	*body_pos = dBodyGetPosition (body.id ());
	double		target_pos[3] = { x, y, 0 };
	double		target_dir[3];
				VoVoV (target_dir, =, target_pos, -,
body_pos);
	double		dist = Vlen (target_dir);
				Vnormalize (target_dir);

	add_target_force (body, target_dir, dist, move_crit_dist_,
		move_max_speed_, move_force_k_);

}