def inv(x):
res = np.zeros(7)
res[3:] = quat.conj(x[3:])
res[:3] = -quat.rotate(res[3:], x[:3])
return res
def inv(x):
res = np.zeros(7)
res[3:] = quat.conj(x[3:])
res[:3] = -quat.rotate(res[3:], x[:3])
return res
def onBeginAnimationStep(self, dt):
# current mu from current plane angle
currentMu = math.tan( self.currentAngle )
if self.counter < 100 : # does not rotate the plane for 100 time steps
self.counter += 1
return 0
# is it a mu we want to test?
if numpy.allclose( self.muToTest, currentMu, 1e-3, 1e-3 ) :
# at the end of 100 time steps, check if the box was sticking or sliding
self.counter = 0
self.muToTest += 0.1
# look at the box velocity along its x-axis
localbox = Quaternion.rotate(Quaternion.conj( Frame.Frame( shared.plane.position[0] ).rotation ),
shared.box.velocity[0][:3])
vel = localbox[0]
#print 'plane/ground angle:', self.currentAngle
#print 'velocity:',vel
#print shared.box.position[0], shared.box.velocity[0][:3]
#print vel, currentMu, shared.mu
testVel = (vel > 1e-1)
if testVel:
testMu = (currentMu>=shared.mu-1e-2)
else:
testMu = (currentMu>=shared.mu)
EXPECT_FALSE( testVel ^ testMu, str(vel)+' '+str(currentMu)+'mu='+str(shared.mu) ) # xor
#print testVel, testMu
#sys.stdout.flush()
# all finished
if currentMu >= shared.mu + .1:
self.sendSuccess()
# update plane orientation
self.currentAngle += 0.001
q = Quaternion.from_euler( [0,0,-self.currentAngle] )
p = shared.plane.position
p[0] = [0,0,0,q[3],q[2],q[1],q[0]]
shared.plane.position = p
return 0
def onBeginAnimationStep(self, dt):
# current mu from current plane angle
currentMu = math.tan(self.currentAngle)
if self.counter < 100: # does not rotate the plane for 100 time steps
self.counter += 1
return 0
# is it a mu we want to test?
if numpy.allclose(self.muToTest, currentMu, 1e-3, 1e-3):
# at the end of 100 time steps, check if the box was sticking or sliding
self.counter = 0
self.muToTest += 0.1
# look at the box velocity along its x-axis
localbox = Quaternion.rotate(
Quaternion.conj(
Frame.Frame(shared.plane.position[0]).rotation),
shared.box.velocity[0][:3])
vel = localbox[0]
#print 'plane/ground angle:', self.currentAngle
#print 'velocity:',vel
#print shared.box.position[0], shared.box.velocity[0][:3]
# print vel, currentMu, shared.mu
testVel = (vel > 1e-1)
if testVel:
testMu = (currentMu >= shared.mu - 1e-2)
else:
testMu = (currentMu >= shared.mu)
EXPECT_FALSE(testVel ^ testMu,
str(vel) + ' ' + str(currentMu) + 'mu=' +
str(shared.mu)) # xor
#print testVel, testMu
#sys.stdout.flush()
# all finished
if currentMu >= shared.mu + .1:
self.sendSuccess()
# update plane orientation
self.currentAngle += 0.001
q = Quaternion.from_euler([0, 0, -self.currentAngle])
p = shared.plane.position
p[0] = [0, 0, 0, q[3], q[2], q[1], q[0]]
shared.plane.position = p
return 0
def inv(self):
res = Frame()
res.rotation = quat.conj(self.rotation)
res.translation = -quat.rotate(res.rotation, self.translation)
return res
def inv(self):
res = Frame()
res.rotation = quat.conj( self.rotation )
res.translation = - quat.rotate(res.rotation, self.translation)
return res
