def applyStep(self,step):
v = self.getVelocity()
v.set(0.0,0.0)
#self.angularVelocity = 0.0
for mobile in self.mobiles:
vmobile = mobile.getVelocity().copy()
v.add(vmobile)
#vmobile = mobile.getVelocity()
om = mobile.getPhysicalPosition().copy()
om.sub(self.getPosition())
#om.display()
vmobile.mulScalar(mobile.getMass())
if om.length() != 0.0 :
av = Vector2d.vtangent(om,vmobile) / om.length()
av = -av
else :
av = 0.0
if om.getX() < 0.0:
av = -av
if vmobile.getY() > 0.0:
av = -av
#print "ANGULAR VELOCITY AV=%f OM=%f MASS=%f"%(av,om.length(),mobile.getMass())
self.angularVelocity += av * 0.5
v.mulScalar(step)
self.getPosition().add(v)
#print "ANGULAR VELOCITY=%f"%(self.angularVelocity)
self.angle += self.angularVelocity * step*0.001
#self.angularVelocity = self.angularVelocity / 2.0
self.updateMobiles()
#print 'TOTO: %d %d'%(self.stablecount,self.stable)
#print '%f - %f'%(self.angle,self.oldangle)
#print '%d'%int(fabs(self.getPosition().getX()-self.oldposition.getX()))
#print '%d - %f - %f'%(int(fabs(self.getPosition().getY()-self.oldposition.getY())),self.getPosition().getY(),self.oldposition.getY())
if int(self.angle) == int(self.oldangle) and int(fabs(self.getPosition().getX()-self.oldposition.getX())) < 1 and int(fabs(self.getPosition().getY()-self.oldposition.getY())) < 1 :
self.stablecount += 1
if self.stablecount > 180:
self.stable = 1
else :
self.stablecount = 0
self.oldposition.set(self.getPosition().getX(),self.getPosition().getY())
self.oldangle = self.angle
