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