[ODE] How to keep capsule stand and how to make it hover over below objects at a certain height?

[michael kapelko]

I forgot to mention, that I do everything ray related inside 
NearCallback. Here it is:

void NearCallback (void *data, dGeomID o1, dGeomID o2) {
         const int ALPHA = 20;

         // Ray first
         if (dGeomGetClass (o1) == dRayClass) {
                 dContactGeom cont;
                 if (dCollide (player->phys_pl->player_ray, o2, 1, 
&cont, sizeof (dContactGeom))) {
                         const dReal *pos = dGeomGetPosition 
(player->phys_pl->player_geom);                        float distance = 
pos[1] - 20 - cont.pos[1];
                         game_core.UpdateRayLength (distance);
                         if (distance < 15 && distance > -5) {
                                 const dReal *poss = dBodyGetPosition 
(player->phys_pl->player_body);
                                 float diff = 15 - abs (distance);
                                 dBodySetPosition 
(player->phys_pl->player_body, poss[0], poss[1] + diff,
                                                 poss[2]);
                                 //dBodyAddForce 
(player->phys_pl->player_body, 0, diff * ALPHA, 0);
                         }
                 }
                 return;
         }
         if (dGeomGetClass (o2) == dRayClass) {
                 dContactGeom cont;
                 if (dCollide (player->phys_pl->player_ray, o1, 1, 
&cont, sizeof (dContactGeom))) {
                         const dReal *pos = dGeomGetPosition 
(player->phys_pl->player_geom);                        float distance = 
pos[1] - 20 - cont.pos[1];
                         game_core.UpdateRayLength (distance);
                         if (distance < 15 && distance > -5) {
                                 const dReal *poss = dBodyGetPosition 
(player->phys_pl->player_body);
                                 float diff = 15 - abs (distance);
                                 dBodySetPosition 
(player->phys_pl->player_body, poss[0], poss[1] + diff,
                                                 poss[2]);
                                 //dBodyAddForce 
(player->phys_pl->player_body, 0, diff * ALPHA, 0);
                         }
                 }
                 return;
         }

         dBodyID b1 = dGeomGetBody (o1);
         dBodyID b2 = dGeomGetBody (o2);
         // If bodies are connected with a joint, then no collision 
detection needed
         if (b1 && b2 && dAreConnected (b1, b2))
                 return;

         // Maximum possible contacts
         const int N = 4;
         dContact contact[N];
         // How much contacts do we actually have?
         int n = dCollide (o1, o2, N, &contact[0].geom, sizeof (dContact));
         if (n > 0)
                 for (int i = 0; i < n; i++) {
                         // What these parameters mean, you can read in 
ODE Manual
                         contact[i].surface.mode = dContactSlip1 | 
dContactSlip2 |
                                 dContactSoftERP | dContactSoftCFM | 
dContactApprox1;
                         contact[i].surface.mu = dInfinity;
                         contact[i].surface.slip1 = 0.0;
                         contact[i].surface.slip2 = 0.0;
                         contact[i].surface.soft_erp = 0.4;
                         contact[i].surface.soft_cfm = 0.01;
                         dJointID c = dJointCreateContact (world, 
contact_group,
                                         &contact[i]);
                         dJointAttach (c, dGeomGetBody (contact[i].geom.g1),
                                         dGeomGetBody (contact[i].geom.g2));
                 }
}