+ _ground_effect_altitude=findGroundEffectAltitude(ground_cb,s,
+ _groundeffectpos[0],_groundeffectpos[1],
+ _groundeffectpos[2],_groundeffectpos[3]);
+ testForRotorGroundContact(ground_cb,s);
+}
+
+void Rotor::testForRotorGroundContact(Ground * ground_cb,State *s)
+{
+ int i;
+ for (i=0;i<_num_ground_contact_pos;i++)
+ {
+ double pt[3],h;
+ s->posLocalToGlobal(_ground_contact_pos[i], pt);
+
+ // Ask for the ground plane in the global coordinate system
+ double global_ground[4];
+ float global_vel[3];
+ ground_cb->getGroundPlane(pt, global_ground, global_vel);
+ // find h, the distance to the ground
+ // The ground plane transformed to the local frame.
+ float ground[4];
+ s->planeGlobalToLocal(global_ground, ground);
+
+ h = ground[3] - Math::dot3(_ground_contact_pos[i], ground);
+ // Now h is the distance from _ground_contact_pos[i] to ground
+ if (h<0)
+ {
+ _balance1 -= (-h)/_diameter/_num_ground_contact_pos;
+ _balance1 = (_balance1<-1)?-1:_balance1;
+ }
+ }
+}
+float Rotor::findGroundEffectAltitude(Ground * ground_cb,State *s,
+ float *pos0,float *pos1,float *pos2,float *pos3,
+ int iteration,float a0,float a1,float a2,float a3)
+{
+ float a[5];
+ float *p[5],pos4[3];
+ a[0]=a0;
+ a[1]=a1;
+ a[2]=a2;
+ a[3]=a3;
+ a[4]=-1;
+ p[0]=pos0;
+ p[1]=pos1;
+ p[2]=pos2;
+ p[3]=pos3;
+ p[4]=pos4;
+ Math::add3(p[0],p[2],p[4]);
+ Math::mul3(0.5,p[4],p[4]);//the center
+
+ float mina=100*_diameter;
+ float suma=0;
+ for (int i=0;i<5;i++)
+ {
+ if (a[i]==-1)//in the first iteration,(iteration==0) no height is
+ //passed to this function, these missing values are
+ //marked by ==-1
+ {
+ double pt[3];
+ s->posLocalToGlobal(p[i], pt);
+
+ // Ask for the ground plane in the global coordinate system
+ double global_ground[4];
+ float global_vel[3];
+ ground_cb->getGroundPlane(pt, global_ground, global_vel);
+ // find h, the distance to the ground
+ // The ground plane transformed to the local frame.
+ float ground[4];
+ s->planeGlobalToLocal(global_ground, ground);
+
+ a[i] = ground[3] - Math::dot3(p[i], ground);
+ // Now a[i] is the distance from p[i] to ground
+ }
+ suma+=a[i];
+ if (a[i]<mina)
+ mina=a[i];
+ }
+ if (mina>=10*_diameter)
+ return mina; //the ground effect will be zero
+
+ //check if further recursion is neccessary
+ //if the height does not differ more than 20%, than
+ //we can return then mean height, if not split
+ //zhe square to four parts and calcualte the height
+ //for each part
+ //suma * 0.2 is the mean
+ //0.15 is the maximum allowed difference from the mean
+ //to the height at the center
+ if ((iteration>2)
+ ||(Math::abs(suma*0.2-a[4])<(0.15*0.2*suma*(1<<iteration))))
+ return suma*0.2;
+ suma=0;
+ float pc[4][3],ac[4]; //pc[i]=center of pos[i] and pos[(i+1)&3]
+ for (int i=0;i<4;i++)
+ {
+ Math::add3(p[i],p[(i+1)&3],pc[i]);
+ Math::mul3(0.5,pc[i],pc[i]);
+ double pt[3];
+ s->posLocalToGlobal(pc[i], pt);
+
+ // Ask for the ground plane in the global coordinate system
+ double global_ground[4];
+ float global_vel[3];
+ ground_cb->getGroundPlane(pt, global_ground, global_vel);
+ // find h, the distance to the ground
+ // The ground plane transformed to the local frame.
+ float ground[4];
+ s->planeGlobalToLocal(global_ground, ground);
+
+ ac[i] = ground[3] - Math::dot3(p[i], ground);
+ // Now ac[i] is the distance from pc[i] to ground
+ }
+ return 0.25*
+ (findGroundEffectAltitude(ground_cb,s,p[0],pc[1],p[4],pc[3],
+ iteration+1,a[0],ac[0],a[4],ac[3])
+ +findGroundEffectAltitude(ground_cb,s,p[1],pc[0],p[4],pc[1],
+ iteration+1,a[1],ac[0],a[4],ac[1])
+ +findGroundEffectAltitude(ground_cb,s,p[2],pc[1],p[4],pc[2],
+ iteration+1,a[2],ac[1],a[4],ac[2])
+ +findGroundEffectAltitude(ground_cb,s,p[3],pc[2],p[4],pc[3],
+ iteration+1,a[3],ac[2],a[4],ac[3])
+ );