def ik_leg(self):
floor_pos = self.get_floor_spot()
target_pos = self._get_target_pos()
hip_pos = self.hip_bone.get_pos(self.foot_ref)
if floor_pos and target_pos.y < floor_pos.y:
floor_pos.x = target_pos.x
target_vector = hip_pos - floor_pos
self.is_on_ground = True
else:
target_vector = hip_pos - target_pos
self.is_on_ground = False
#turn off the ground for debugging
#target_vector = hip_pos - target_pos
pt_length = target_vector.length()
if .01 < pt_length < (TOP_LEG_LENGTH + BOTTOM_LEG_LENGTH +.1):
tt_angle_cos = (math.pow(TOP_LEG_LENGTH, 2) + math.pow(pt_length, 2) - math.pow(BOTTOM_LEG_LENGTH,2))/(2*TOP_LEG_LENGTH*pt_length)
try:
target_top_angle = rad2Deg(math.acos(tt_angle_cos))
except ValueError:
return
target_vector.normalize()
self.hip_bone.get_relative_vector(self.foot_ref, target_vector)
delta = target_vector.get_xy().signed_angle_deg(Vec3.unitY().get_xy())
self.top_bone.set_p(90 - target_top_angle + delta)
tb_angle_cos = ((math.pow(TOP_LEG_LENGTH, 2) + math.pow(BOTTOM_LEG_LENGTH, 2) - math.pow(pt_length,2))/(2*TOP_LEG_LENGTH*BOTTOM_LEG_LENGTH))
target_bottom_angle = rad2Deg(math.acos(tb_angle_cos))
self.bottom_bone.set_p((180 - target_bottom_angle) * -1)
else:
return
def commonMove(self):
if not self.lastThinkTime:
self.lastThinkTime = globalClock.getFrameTime()
dt = globalClock.getFrameTime() - self.lastThinkTime
self.oldpos = self.suit.getPos()
pos = self.suit.getPos()
pos += self.velocity * dt
self.suit.setPos(pos)
self.seeFriends()
acc = Vec3(0, 0, 0)
self.accumulate(acc, self.getTargetVector())
if self.numFlockmatesSeen > 0:
keepDistanceVector = self.keepDistance()
oldAcc = Vec3(acc)
self.accumulate(acc, keepDistanceVector)
if self.cogIndex == 0:
pass
if acc.length() > self.maxAcceleration:
acc.normalize()
acc *= self.maxAcceleration
self.oldVelocity = self.velocity
self.velocity += acc
if self.velocity.length() > self.maxVelocity:
self.velocity.normalize()
self.velocity *= self.maxVelocity
forwardVec = Vec3(1, 0, 0)
heading = rad2Deg(math.atan2(self.velocity[1], self.velocity[0]))
heading -= 90
self.suit.setH(heading)
def loadTrainTrack(self, x1, y1, x2, y2, zAdj=0):
turnTable = loader.loadModel(
'phase_4/models/minigames/trolley_game_turntable')
trainPart = turnTable.find('**/track_a2')
trackHeight = 0.03
trainTrack = render.attachNewNode('trainTrack%d%d%d%d' %
(x1, y1, x2, y2))
trainTrack.setPos(x1, y1, trackHeight)
xDiff = abs(x2 - x1)
yDiff = abs(y2 - y1)
angleInRadians = math.atan((float(y2) - y1) / (x2 - x1))
angle = rad2Deg(angleInRadians)
desiredLength = math.sqrt(xDiff * xDiff + yDiff * yDiff)
lengthToGo = desiredLength
partIndex = 0
lengthCovered = 0
while lengthToGo > self.fullLength / 2.0:
onePart = trainPart.copyTo(trainTrack)
onePart.setX(lengthCovered)
lengthToGo -= self.fullLength
lengthCovered += self.fullLength
trainTrack.setH(angle)
newX = x1 + (x2 - x1) / 2.0
newY = y1 + (y2 - y1) / 2.0
trainTrack.setPos(x1, y1, trackHeight + zAdj)
turnTable.removeNode()
return trainTrack
def vecToYaw(vec):
"""Converts the given vector to a yaw angle in degrees."""
return rad2Deg(math.atan2(vec[1], vec[0])) - 90
def setSpeed(self, forwardSpeed, rotateSpeed, strafeSpeed=0.0):
if self.forceRunSpeed:
forwardSpeed = CIGlobals.RunCutOff
self.forwardSpeed = forwardSpeed
self.rotateSpeed = rotateSpeed
self.strafeSpeed = strafeSpeed
action = None
if self.standWalkRunReverse != None:
rotateCutOff = CIGlobals.RotateCutOff if not self.isLocalAvatar(
) else CIGlobals.WalkCutOff
if strafeSpeed < CIGlobals.StrafeCutOff and strafeSpeed > -CIGlobals.StrafeCutOff:
self.resetTorsoRotation()
if forwardSpeed >= CIGlobals.RunCutOff:
action = CIGlobals.RUN_INDEX
elif forwardSpeed > CIGlobals.WalkCutOff:
action = CIGlobals.WALK_INDEX
elif forwardSpeed < -CIGlobals.WalkCutOff:
action = CIGlobals.REVERSE_INDEX
elif abs(rotateSpeed) > rotateCutOff:
action = CIGlobals.WALK_INDEX
elif abs(strafeSpeed) > CIGlobals.RunCutOff:
action = CIGlobals.RUN_INDEX
elif abs(strafeSpeed) > CIGlobals.WalkCutOff:
action = CIGlobals.WALK_INDEX
else:
action = CIGlobals.STAND_INDEX
if abs(strafeSpeed) > CIGlobals.WalkCutOff:
spine = self.find("**/def_spineB")
if spine.isEmpty():
spine = self.controlJoint(None, "torso", "def_spineB")
movementVec = Vec3(strafeSpeed, forwardSpeed, 0)
movementVec.normalize()
movementAngle = rad2Deg(
math.atan2(movementVec[1], movementVec[0])) - 90.0
if action == CIGlobals.REVERSE_INDEX:
movementAngle -= 180
spine.setH(-movementAngle)
self.getPart('legs').setH(movementAngle)
anim, rate = self.standWalkRunReverse[action]
if anim != self.playingAnim or rate != self.playingRate or self.forcedTorsoAnim != self.lastForcedTorsoAnim:
self.playingAnim = anim
self.playingRate = rate
self.lastForcedTorsoAnim = self.forcedTorsoAnim
if self.forcedTorsoAnim is None:
self.loop(anim)
else:
# Whatever happens to the legs should also happen on the pants.
self.loop(anim, partName='torso-pants')
self.loop(anim, partName='legs')
self.setPlayRate(rate, anim)
return action
def vecToYaw(vec):
return rad2Deg(math.atan2(vec[1], vec[0])) - 90
