我们从Python开源项目中，提取了以下50个代码示例，用于说明如何使用math.atan2()。
def __init__(self, sprites, dest_pos, speed=None): """ Initializes an object of SpriteMove class. :param sprites: list of card sprites to be moved :param dest_pos: tuple with coordinates (x,y) of destination position :param speed: integer number, on how many pixels card(s) should move per frame. If not specified (None), "move_speed" value from the config json is used. """ self.sprites = sprites self.dest_pos = dest_pos for sprite in self.sprites: sprite.start_pos = sprite.pos sprite.angle = math.atan2(dest_pos[1] - sprite.start_pos[1], dest_pos[0] - sprite.start_pos[0]) sprite.distance = SpriteMove.calc_distance(dest_pos, sprite.start_pos) if speed is None: sprite.speed = CardSprite.card_json["move_speed"] else: sprite.speed = speed sprite.completed = False
def ComputeCoverage(sigma_points, bias): def ang(p): v = rotvecquat(vector.sub(p.compass, bias), vec2vec2quat(p.down, [0, 0, 1])) return math.atan2(v[1], v[0]) angles = sorted(map(ang, sigma_points)) #print 'angles', angles max_diff = 0 for i in range(len(angles)): diff = -angles[i] j = i+1 if j == len(angles): diff += 2*math.pi j = 0 diff += angles[j] max_diff = max(max_diff, diff) return max_diff
def eci_to_latlon(pos, phi_0=0): (x, y, z) = pos rg = (x*x + y*y + z*z)**0.5 z = z/rg if abs(z) > 1.0: z = int(z) lat = degrees(asin(z)) lon = degrees(atan2(y, x)-phi_0) if lon > 180: lon -= 360 elif lon < -180: lon += 360 assert -90 <= lat <= 90 assert -180 <= lon <= 180 return lat, lon
def quat2euler(q0, q1, q2, q3): """ Converts quaterion to (pitch, yaw, roll). Values are in -PI to PI range. """ qq0, qq1, qq2, qq3 = q0**2, q1**2, q2**2, q3**2 xa = qq0 - qq1 - qq2 + qq3 xb = 2 * (q0 * q1 + q2 * q3) xn = 2 * (q0 * q2 - q1 * q3) yn = 2 * (q1 * q2 + q0 * q3) zn = qq3 + qq2 - qq0 - qq1 pitch = atan2(xb , xa) yaw = atan2(xn , sqrt(1 - xn**2)) roll = atan2(yn , zn) return pitch, yaw, roll
def compute_side(self, x, y): """ Computes which sides of dpad are supposed to be active """ ## dpad(up, down, left, right) ## dpad8(up, down, left, right, upleft, upright, downleft, downright) side = self.SIDE_NONE if x*x + y*y > self.MIN_DISTANCE_P2: # Compute angle from center of pad to finger position angle = (atan2(x, y) * 180.0 / PI) + 180 # Translate it to index index = 0 for a1, a2, i in self.ranges: if angle >= a1 and angle < a2: index = i break side = self.SIDES[index] return side
def mode_LINEAR(self, x, y, range): """ Input value is scaled, so entire output range is covered by reduced input range of deadzone. """ if y == 0: # Small optimalization for 1D input, for example trigger return copysign( clamp( 0, ((x - self.lower) / (self.upper - self.lower)) * range, range), x ), 0 distance = clamp(self.lower, sqrt(x*x + y*y), self.upper) distance = (distance - self.lower) / (self.upper - self.lower) * range angle = atan2(x, y) return distance * sin(angle), distance * cos(angle)
def getStatistics(circle_x, circle_y, bar1_x, bar1_y, bar2_x, bar2_y): out = [0, 0, 0] midX = GLOBAL_WIDTH / 2 midY = GLOBAL_HEIGHT / 2 dx = midX - circle_x dy = midY - circle_y rads = atan2(-dy, dx) rads %= 2*pi angle = degrees(rads) if (bar1_x - circle_x)**2 != 0: p1Distance = sqrt((bar1_y - circle_y)**2 / (bar1_x - circle_x)**2) if (bar2_x - circle_x)**2 != 0: p2Distance = sqrt((bar2_y - circle_y)**2 / (bar2_x - circle_x)**2) out[0] = angle out[1] = p1Distance out[2] = p2Distance return out #determines how to move padel based on neural net input
def get_earth_dist(pt_a, pt_b=None): if type(pt_a) is str or pt_b is None: return 'unkn' # No location set log.debug("Calculating distance from {} to {}".format(pt_a, pt_b)) lat_a = radians(pt_a[0]) lng_a = radians(pt_a[1]) lat_b = radians(pt_b[0]) lng_b = radians(pt_b[1]) lat_delta = lat_b - lat_a lng_delta = lng_b - lng_a a = sin(lat_delta / 2) ** 2 + cos(lat_a) * cos(lat_b) * sin(lng_delta / 2) ** 2 c = 2 * atan2(sqrt(a), sqrt(1 - a)) radius = 6373000 # radius of earth in meters if config['UNITS'] == 'imperial': radius = 6975175 # radius of earth in yards dist = c * radius return dist # Return the time as a string in different formats
def pose_2_xyzrpw(H): """Calculates the equivalent position and euler angles ([x,y,z,r,p,w] array) of the given pose according to the following operation: Note: transl(x,y,z)*rotz(w*pi/180)*roty(p*pi/180)*rotx(r*pi/180) See also: xyzrpw_2_pose() :param H: pose :type H: :class:`.Mat`""" x = H[0,3] y = H[1,3] z = H[2,3] if (H[2,0] > (1.0 - 1e-6)): p = -pi/2 r = 0 w = math.atan2(-H[1,2],H[1,1]) elif H[2,0] < -1.0 + 1e-6: p = pi/2 r = 0 w = math.atan2(H[1,2],H[1,1]) else: p = math.atan2(-H[2,0],sqrt(H[0,0]*H[0,0]+H[1,0]*H[1,0])) w = math.atan2(H[1,0],H[0,0]) r = math.atan2(H[2,1],H[2,2]) return [x, y, z, r*180/pi, p*180/pi, w*180/pi]
def Pose_2_KUKA(H): """Converts a pose (4x4 matrix) to a Kuka target :param H: pose :type H: :class:`.Mat`""" x = H[0,3] y = H[1,3] z = H[2,3] if (H[2,0]) > (1.0 - 1e-6): p = -pi/2 r = 0 w = atan2(-H[1,2],H[1,1]) elif (H[2,0]) < (-1.0 + 1e-6): p = pi/2 r = 0 w = atan2(H[1,2],H[1,1]) else: p = atan2(-H[2,0],sqrt(H[0,0]*H[0,0]+H[1,0]*H[1,0])) w = atan2(H[1,0],H[0,0]) r = atan2(H[2,1],H[2,2]) return [x, y, z, w*180/pi, p*180/pi, r*180/pi]
def get_center_of_nodes(nodes): """Helper function to get center coordinates of a group of nodes """ x = 0 y = 0 z = 0 for node in nodes: lat = radians(float(node.lat)) lon = radians(float(node.lon)) x += cos(lat) * cos(lon) y += cos(lat) * sin(lon) z += sin(lat) x = float(x / len(nodes)) y = float(y / len(nodes)) z = float(z / len(nodes)) center_lat = degrees(atan2(z, sqrt(x * x + y * y))) center_lon = degrees(atan2(y, x)) return center_lat, center_lon
def get_crow_fly_distance(from_tuple, to_tuple): """ Uses the Haversine formmula to compute distance (https://en.wikipedia.org/wiki/Haversine_formula#The_haversine_formula) """ lat1, lon1 = from_tuple lat2, lon2 = to_tuple lat1 = float(lat1) lat2 = float(lat2) lon1 = float(lon1) lon2 = float(lon2) radius = 6371 # km dlat = math.radians(lat2 - lat1) dlon = math.radians(lon2 - lon1) a = math.sin(dlat / 2) * math.sin(dlat / 2) + math.cos(math.radians(lat1)) * \ math.cos(math.radians(lat2)) * math.sin(dlon / 2) * math.sin(dlon / 2) c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a)) d = radius * c return d * 1000 # meters
def _intersect_circle(self, center, radius, x): """ upper intersection of line parallel to y axis and a circle where line is given by x origin circle by center, radius as float return float y of upper intersection point, float angle """ dx = x - center.x d = (radius * radius) - (dx * dx) if d <= 0: if x > center.x: return center.y, 0 else: return center.y, pi else: y = sqrt(d) return center.y + y, atan2(y, dx)
def _intersect_elipsis(self, center, radius, x): """ upper intersection of line parallel to y axis and an ellipsis where line is given by x origin circle by center, radius.x and radius.y semimajor and seminimor axis (half width and height) as float return float y of upper intersection point, float angle """ dx = x - center.x d2 = dx * dx A = 1 / radius.y / radius.y C = d2 / radius.x / radius.x - 1 d = - 4 * A * C if d <= 0: if x > center.x: return center.y, 0 else: return center.y, pi else: y0 = sqrt(d) / 2 / A d = (radius.x * radius.x) - d2 y = sqrt(d) return center.y + y0, atan2(y, dx)
def rotate(self, a): """ Rotate center so we rotate ccw arround p0 """ ca = cos(a) sa = sin(a) rM = Matrix([ [ca, -sa], [sa, ca] ]) p0 = self.p0 self.c = p0 + rM * (self.c - p0) dp = p0 - self.c self.a0 = atan2(dp.y, dp.x) return self # make offset for line / arc, arc / arc
def Encode(self, normal): x = normal[0] y = normal[1] z = normal[2] # normalize l = math.sqrt((x*x) + (y*y) + (z*z)) if l == 0: return 0 x = x/l y = y/l z = z/l if (x == 0.0) & (y == 0.0) : if z > 0.0: return 0 else: return (128 << 8) lng = math.acos(z) * 255 / (2 * math.pi) lat = math.atan2(y, x) * 255 / (2 * math.pi) retval = ((int(lat) & 0xFF) << 8) | (int(lng) & 0xFF) return retval
def recordCoordinate(item, p): x, y, z = p # precalculate angle from wheel center (0, -20, 13.9) dy = y + 20 dz = z - 14.75 theta = atan2(dy, dz) if theta < 0: theta = 2 * pi + theta r = sqrt(dy * dy + dz * dz) zr = cos(theta) yr = sin(theta) item['x'] = x item['y'] = y item['z'] = z item['theta'] = theta item['r'] = r # for backwards compatibility, remove in future? item['coord'] = (x, y, z, theta, r, zr, yr)
def drawBallVelocity(self, painter): ballPos = self.ballOdom.pose.pose.position ballVel = self.ballOdom.twist.twist.linear if math.hypot(ballVel.x, ballVel.y) < 1.0: return angleOfSpeed = math.atan2(ballVel.y, ballVel.x) paintDist = 10.0 velPosX = paintDist * math.cos(angleOfSpeed) + ballPos.x velPosY = paintDist * math.sin(angleOfSpeed) + ballPos.y ballPosPoint = self.convertToDrawWorld(ballPos.x, ballPos.y) velPosPoint = self.convertToDrawWorld(velPosX, velPosY) painter.setPen(QPen(QColor(102,0,255),2)) painter.drawLine(ballPosPoint, velPosPoint)
def getVelocity(self, cur, goal, dT): desired = Pose() d = self.getGoalDistance(cur, goal) a = atan2(goal.y - cur.y, goal.x - cur.x) - cur.theta b = cur.theta + a - goal.theta if abs(d) < self.linearTolerance: desired.xVel = 0 desired.thetaVel = -self.kB * b else: desired.xVel = self.kP * d desired.thetaVel = self.kA*a + self.kB*b # Adjust velocities if linear or angular rates or accel too high. # TBD return desired
def update_velocity(self): HALF_WIDTH = const.WINDOW_WIDTH//2 - self.width//2 if (const.BOMB_X+self.width/2) - self.dest_x == 0: self.vel_x = 0 self.vel_y = self.SPEED elif (const.BOMB_Y+self.height/2) - self.dest_y == 0: if self.dest_x <= const.BOMB_X: ''' note the equals ''' self.vel_x = -self.SPEED self.vel_y = 0 elif self.dest_x > const.BOMB_X: self.vel_x = self.SPEED self.vel_y = 0 else: '''(x+width/2, y+height/2) is the centre of the projectile image''' dy = self.dest_y - (const.BOMB_Y+self.height/2) dx = self.dest_x - (const.BOMB_X+self.width/2) radians = math.atan2(dy, dx) self.vel_y = math.sin(radians)*self.SPEED self.vel_x = math.cos(radians)*self.SPEED degrees = radians * 180.0 / math.pi
def update_velocity(self): HALF_WIDTH = const.WINDOW_WIDTH//2 - self.width//2 if (const.FROST_POTION_X+self.width/2) - self.dest_x == 0: self.vel_x = 0 self.vel_y = self.SPEED elif (const.FROST_POTION_Y+self.height/2) - self.dest_y == 0: if self.dest_x <= const.BOMB_X: ''' note the equals ''' self.vel_x = -self.SPEED self.vel_y = 0 elif self.dest_x > const.BOMB_X: self.vel_x = self.SPEED self.vel_y = 0 else: dy = self.dest_y - (const.FROST_POTION_Y+self.height/2) dx = self.dest_x - (const.FROST_POTION_X+self.width/2) radians = math.atan2(dy, dx)# + math.pi/2 self.vel_y = math.sin(radians)*self.SPEED self.vel_x = math.cos(radians)*self.SPEED degrees = radians * 180.0 / math.pi
def gps_newpos(lat, lon, bearing, distance): """Extrapolate latitude/longitude given a heading and distance thanks to http://www.movable-type.co.uk/scripts/latlong.html . """ from math import sin, asin, cos, atan2, radians, degrees lat1 = radians(lat) lon1 = radians(lon) brng = radians(bearing) dr = distance / radius_of_earth lat2 = asin(sin(lat1) * cos(dr) + cos(lat1) * sin(dr) * cos(brng)) lon2 = lon1 + atan2(sin(brng) * sin(dr) * cos(lat1), cos(dr) - sin(lat1) * sin(lat2)) return (degrees(lat2), degrees(lon2))
def robot_listener(self): ''' rospy.wait_for_service('spawn') spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn) spawner(4, 2, 0, 'turtle2') ''' robot_vel_pub = rospy.Publisher('robot_0/cmd_vel', geometry_msgs.msg.Twist, queue_size=1) rate = rospy.Rate(10.0) while not rospy.is_shutdown(): try: (trans,rot) = self.listener.lookupTransform('/robot_3', '/robot_0', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue angular = 4 * math.atan2(trans[1], trans[0]) linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2) cmd = geometry_msgs.msg.Twist() cmd.linear.x = linear cmd.angular.z = angular robot_vel_pub.publish(cmd) rate.sleep()
def ComplementaryFilter(gyrData, accData, pitch, roll): pitchAcc = 0.0 rollAcc = 0.0 # Integrate the gyroscope data -> int(angularSpeed) = angle pitch += (gyrData[0] / GYROSCOPE_SENSITIVITY) * \ dt # Angle around the X-axis roll -= (gyrData[1] / GYROSCOPE_SENSITIVITY) * \ dt # Angle around the Y-axis # Compensate for drift with accelerometer data if !bullshit # Sensitivity = -2 to 2 G at 16Bit -> 2G = 32768 && 0.5G = 8192 forceMagnitudeApprox = abs(accData[0]) + abs(accData[1]) + abs(accData[2]) if (forceMagnitudeApprox > 8192 and forceMagnitudeApprox < 32768): # Turning around the X axis results in a vector on the Y-axis pitchAcc = math.atan2(accData[1], accData[2]) * 180 / M_PI pitch = pitch * 0.98 + pitchAcc * 0.02 # Turning around the Y axis results in a vector on the X-axis rollAcc = math.atan2(accData[0], accData[2]) * 180 / M_PI roll = roll * 0.98 + rollAcc * 0.02 return pitch, roll
def heading(self): (x, y, z) = self.raw_data() headingRad = math.atan2(y, x) headingRad += self.__declination # Correct for reversed heading if headingRad < 0: headingRad += 2 * math.pi # Check for wrap and compensate elif headingRad > 2 * math.pi: headingRad -= 2 * math.pi # Convert to degrees from radians headingDeg = headingRad * 180 / math.pi return headingDeg
def _mat3_to_vec_roll(mat): """ Function to convert a 3x3 rotation matrix to a rotation axis and a roll angle along this axis Python port of the C function defined in armature.c Thanks to blenderartists.org user vida_vida :param mat: :return: """ from ..properties.globals import global_properties vec = mat.col[1] * global_properties.bone_length.get(bpy.context.scene) vecmat = _vec_roll_to_mat3(mat.col[1], 0) vecmatinv = vecmat.inverted() rollmat = vecmatinv * mat roll = math.atan2(rollmat[0][2], rollmat[2][2]) return vec, roll
def ExtendToMont(self,Geom, dist=4):#Retorna geometria com linha estendida #x1,y1,x2,y2 poli=Geom.asPolyline() pto1=poli[0] pto2=poli[1] x1=pto1.x() y1=pto1.y() x2=pto2.x() y2=pto2.y() Alfa=math.atan2(y2-y1,x2-x1) dx=dist*math.cos(Alfa) dy=dist*math.sin(Alfa) xp=x1-dx yp=y1-dy pto1_est=QgsPoint(xp,yp) newGeo=QgsGeometry.fromPolyline([pto1_est,pto2]) return newGeo
def yaw(self): return math.atan2(2*(self.w*self.z() + self.x()*self.y()), 1 - 2*(self.y()*self.y() + self.z()*self.z()))
def roll(self): return math.atan2(2*(self.w*self.x() + self.z()*self.y()), 1 - 2*(self.y()*self.y() + self.x()*self.x()))
def log(q): im = q.im() imn = np.linalg.norm(im) n = math.atan2(imn, q.w) if(abs(n) < 1e-6): return 2*im else: return 2*(n/imn)*im
def getValuesFromPose(self, P): '''return the virtual values of the pots corresponding to the pose P''' vals = [] grads = [] for i, r, l, placement, attach_p in zip(range(3), self.rs, self.ls, self.placements, self.attach_ps): #first pot axis a = placement.rot * col([1, 0, 0]) #second pot axis b = placement.rot * col([0, 1, 0]) #string axis c = placement.rot * col([0, 0, 1]) #attach point on the joystick p_joystick = P * attach_p v = p_joystick - placement.trans va = v - dot(v, a)*a vb = v - dot(v, b)*b #angles of the pots alpha = math.atan2(dot(vb, a), dot(vb, c)) beta = math.atan2(dot(va, b), dot(va, c)) vals.append(alpha) vals.append(beta) #calculation of the derivatives dv = np.bmat([-P.rot.mat() * quat.skew(attach_p), P.rot.mat()]) dva = (np.eye(3) - a*a.T) * dv dvb = (np.eye(3) - b*b.T) * dv dalpha = (1/dot(vb,vb)) * (dot(vb,c) * a.T - dot(vb,a) * c.T) * dvb dbeta = (1/dot(va,va)) * (dot(va,c) * b.T - dot(va,b) * c.T) * dva grads.append(dalpha) grads.append(dbeta) return (col(vals), np.bmat([[grads]]))
def drag_bone(self,context, event ,bone=None): ### math.atan2(0.5, 0.5)*180/math.pi if bone != None: bone.hide = False mouse_vec_norm = (self.cursor_location - self.mouse_click_vec).normalized() mouse_vec = (self.cursor_location - self.mouse_click_vec) angle = (math.atan2(mouse_vec_norm[0], mouse_vec_norm[2])*180/math.pi) cursor_local = self.armature.matrix_world.inverted() * self.cursor_location cursor_local[1] = 0 if event.shift: if angle > -22.5 and angle < 22.5: ### up bone.tail = Vector((bone.head[0],cursor_local[1],cursor_local[2])) elif angle > 22.5 and angle < 67.5: ### up right bone.tail = (bone.head + Vector((mouse_vec[0],0,mouse_vec[0]))) elif angle > 67.5 and angle < 112.5: ### right bone.tail = Vector((cursor_local[0],cursor_local[1],bone.head[2])) elif angle > 112.5 and angle < 157.5: ### down right bone.tail = (bone.head + Vector((mouse_vec[0],0,-mouse_vec[0]))) elif angle > 157.5 or angle < -157.5: ### down bone.tail = Vector((bone.head[0],cursor_local[1],cursor_local[2])) elif angle > -157.5 and angle < -112.5: ### down left bone.tail = (bone.head + Vector((mouse_vec[0],0,mouse_vec[0]))) elif angle > -112.5 and angle < -67.5: ### left bone.tail = Vector((cursor_local[0],cursor_local[1],bone.head[2])) elif angle > -67.5 and angle < -22.5: ### left up bone.tail = (bone.head + Vector((mouse_vec[0],0,-mouse_vec[0]))) else: bone.tail = cursor_local
def calc_bend_trf(self, bend): """calculate the transformations required to 'bend' the leaf out/up from WP""" normal = self.direction.cross(self.right) theta_pos = atan2(self.position.y, self.position.x) theta_bend = theta_pos - atan2(normal.y, normal.x) bend_trf_1 = Quaternion(Vector([0, 0, 1]), theta_bend * bend) self.direction.rotate(bend_trf_1) self.right.rotate(bend_trf_1) normal = self.direction.cross(self.right) phi_bend = normal.declination() if phi_bend > pi / 2: phi_bend = phi_bend - pi bend_trf_2 = Quaternion(self.right, phi_bend * bend) return bend_trf_1, bend_trf_2
def angle(self, n): # ?? n???? ??????????? r = math.sqrt(sum(x * x for x in self[n:])) a = math.atan2(r, self[n - 1]) if (n == len(self)) and (self[-1] < 0): return math.pi * 2 - a else: return a
def rotation_from_matrix(matrix): """Return rotation angle and axis from rotation matrix. >>> angle = (random.random() - 0.5) * (2*math.pi) >>> direc = numpy.random.random(3) - 0.5 >>> point = numpy.random.random(3) - 0.5 >>> R0 = rotation_matrix(angle, direc, point) >>> angle, direc, point = rotation_from_matrix(R0) >>> R1 = rotation_matrix(angle, direc, point) >>> is_same_transform(R0, R1) True """ R = numpy.array(matrix, dtype=numpy.float64, copy=False) R33 = R[:3, :3] # direction: unit eigenvector of R33 corresponding to eigenvalue of 1 l, W = numpy.linalg.eig(R33.T) i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] if not len(i): raise ValueError("no unit eigenvector corresponding to eigenvalue 1") direction = numpy.real(W[:, i[-1]]).squeeze() # point: unit eigenvector of R33 corresponding to eigenvalue of 1 l, Q = numpy.linalg.eig(R) i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] if not len(i): raise ValueError("no unit eigenvector corresponding to eigenvalue 1") point = numpy.real(Q[:, i[-1]]).squeeze() point /= point[3] # rotation angle depending on direction cosa = (numpy.trace(R33) - 1.0) / 2.0 if abs(direction[2]) > 1e-8: sina = (R[1, 0] + (cosa-1.0)*direction[0]*direction[1]) / direction[2] elif abs(direction[1]) > 1e-8: sina = (R[0, 2] + (cosa-1.0)*direction[0]*direction[2]) / direction[1] else: sina = (R[2, 1] + (cosa-1.0)*direction[1]*direction[2]) / direction[0] angle = math.atan2(sina, cosa) return angle, direction, point
def _flushContour(self): points = self._points nPoints = len(points) if not nPoints: return if points[0][1] == "move": # Open path. indices = range(1, nPoints - 1) elif nPoints > 1: # Closed path. To avoid having to mod the contour index, we # simply abuse Python's negative index feature, and start at -1 indices = range(-1, nPoints - 1) else: # closed path containing 1 point (!), ignore. indices = [] for i in indices: pt, segmentType, dummy, name, kwargs = points[i] if segmentType is None: continue prev = i - 1 next = i + 1 if points[prev][1] is not None and points[next][1] is not None: continue # At least one of our neighbors is an off-curve point pt = points[i][0] prevPt = points[prev][0] nextPt = points[next][0] if pt != prevPt and pt != nextPt: dx1, dy1 = pt[0] - prevPt[0], pt[1] - prevPt[1] dx2, dy2 = nextPt[0] - pt[0], nextPt[1] - pt[1] a1 = math.atan2(dx1, dy1) a2 = math.atan2(dx2, dy2) if abs(a1 - a2) < 0.05: points[i] = pt, segmentType, True, name, kwargs for pt, segmentType, smooth, name, kwargs in points: self._outPen.addPoint(pt, segmentType, smooth, name, **kwargs)
def polar2d(vx, vy, deg=True): "2D Cartesian to Polar conversion" a = atan2(vy, vx) return hypot(vx, vy), (a / DEG if deg else a)
def rotation_from_matrix(matrix): """Return rotation angle and axis from rotation matrix. >>> angle = (random.random() - 0.5) * (2*math.pi) >>> direc = numpy.random.random(3) - 0.5 >>> point = numpy.random.random(3) - 0.5 >>> R0 = rotation_matrix(angle, direc, point) >>> angle, direc, point = rotation_from_matrix(R0) >>> R1 = rotation_matrix(angle, direc, point) >>> is_same_transform(R0, R1) True """ R = numpy.array(matrix, dtype=numpy.float64, copy=False) R33 = R[:3, :3] # direction: unit eigenvector of R33 corresponding to eigenvalue of 1 w, W = numpy.linalg.eig(R33.T) i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] if not len(i): raise ValueError("no unit eigenvector corresponding to eigenvalue 1") direction = numpy.real(W[:, i[-1]]).squeeze() # point: unit eigenvector of R33 corresponding to eigenvalue of 1 w, Q = numpy.linalg.eig(R) i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] if not len(i): raise ValueError("no unit eigenvector corresponding to eigenvalue 1") point = numpy.real(Q[:, i[-1]]).squeeze() point /= point[3] # rotation angle depending on direction cosa = (numpy.trace(R33) - 1.0) / 2.0 if abs(direction[2]) > 1e-8: sina = (R[1, 0] + (cosa-1.0)*direction[0]*direction[1]) / direction[2] elif abs(direction[1]) > 1e-8: sina = (R[0, 2] + (cosa-1.0)*direction[0]*direction[2]) / direction[1] else: sina = (R[2, 1] + (cosa-1.0)*direction[1]*direction[2]) / direction[0] angle = math.atan2(sina, cosa) return angle, direction, point
def rotation_from_matrix(matrix): """Return rotation angle and axis from rotation matrix. >>> angle = (random.random() - 0.5) * (2*math.pi) >>> direc = numpy.random.random(3) - 0.5 >>> point = numpy.random.random(3) - 0.5 >>> R0 = rotation_matrix(angle, direc, point) >>> angle, direc, point = rotation_from_matrix(R0) >>> R1 = rotation_matrix(angle, direc, point) >>> is_same_transform(R0, R1) True """ R = numpy.array(matrix, dtype=numpy.float64, copy=False) R33 = R[:3, :3] # direction: unit eigenvector of R33 corresponding to eigenvalue of 1 w, W = numpy.linalg.eig(R33.T) i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] if not len(i): raise ValueError("no unit eigenvector corresponding to eigenvalue 1") direction = numpy.real(W[:, i[-1]]).squeeze() # point: unit eigenvector of R33 corresponding to eigenvalue of 1 w, Q = numpy.linalg.eig(R) i = numpy.where(abs(numpy.real(w) - 1.0) < 1e-8)[0] if not len(i): raise ValueError("no unit eigenvector corresponding to eigenvalue 1") point = numpy.real(Q[:, i[-1]]).squeeze() point /= point[3] # rotation angle depending on direction cosa = (numpy.trace(R33) - 1.0) / 2.0 if abs(direction[2]) > 1e-8: sina = (R[1, 0] + (cosa-1.0)*direction[0]*direction[1]) / direction[2] elif abs(direction[1]) > 1e-8: sina = (R[0, 2] + (cosa-1.0)*direction[0]*direction[2]) / direction[1] else: sina = (R[2, 1] + (cosa-1.0)*direction[1]*direction[2]) / direction[0] angle = math.atan2(sina, cosa) return angle, direction, point # Function to translate handshape coding to degrees of rotation, adduction, flexion
def midpoint(x1, y1, x2, y2): #Input values as degrees #Convert to radians lat1 = math.radians(x1) lon1 = math.radians(x2) lat2 = math.radians(y1) lon2 = math.radians(y2) bx = math.cos(lat2) * math.cos(lon2 - lon1) by = math.cos(lat2) * math.sin(lon2 - lon1) lat3 = math.atan2(math.sin(lat1) + math.sin(lat2), \ math.sqrt((math.cos(lat1) + bx) * (math.cos(lat1) \ + bx) + by**2)) lon3 = lon1 + math.atan2(by, math.cos(lat1) + Bx) return [round(math.degrees(lat3), 2), round(math.degrees(lon3), 2)]
def _ang(vector): 'Private module function.' return _math.atan2(vector.x, vector.y) % _PI_M_2
def angle(self, other): return math.atan2(self.cross_prod(other).norm(), self.dot_prod(other))
def latitude(point): return Angle.from_radians( math.atan2(point[2], math.sqrt(point[0] * point[0] + point[1] * point[1])) )
def longitude(point): return Angle.from_radians(math.atan2(point[1], point[0]))
def get_distance(self, other): assert self.is_valid() assert other.is_valid() from_lat = self.lat().radians to_lat = other.lat().radians from_lng = self.lng().radians to_lng = other.lng().radians dlat = math.sin(0.5 * (to_lat - from_lat)) dlng = math.sin(0.5 * (to_lng - from_lng)) x = dlat * dlat + dlng * dlng * math.cos(from_lat) * math.cos(to_lat) return Angle.from_radians( 2 * math.atan2(math.sqrt(x), math.sqrt(max(0.0, 1.0 - x))) )
def intersects_lat_edge(cls, a, b, lat, lng): assert is_unit_length(a) assert is_unit_length(b) z = robust_cross_prod(a, b).normalize() if z[2] < 0: z = -z y = robust_cross_prod(z, Point(0, 0, 1)).normalize() x = y.cross_prod(z) assert is_unit_length(x) assert x[2] >= 0 sin_lat = math.sin(lat) if math.fabs(sin_lat) >= x[2]: return False assert x[2] > 0 cos_theta = sin_lat / x[2] sin_theta = math.sqrt(1 - cos_theta * cos_theta) theta = math.atan2(sin_theta, cos_theta) ab_theta = SphereInterval.from_point_pair( math.atan2(a.dot_prod(y), a.dot_prod(x)), math.atan2(b.dot_prod(y), b.dot_prod(x)), ) if ab_theta.contains(theta): isect = x * cos_theta + y * sin_theta if lng.contains(math.atan2(isect[1], isect[0])): return True if ab_theta.contains(-theta): isect = x * cos_theta - y * sin_theta if lng.contains(math.atan2(isect[1], isect[0])): return True return False
def distance(origin, destination): """Determine distance between 2 sets of [lat,lon] in km""" lat1, lon1 = origin lat2, lon2 = destination radius = 6371 # km dlat = math.radians(lat2 - lat1) dlon = math.radians(lon2 - lon1) a = (math.sin(dlat / 2) * math.sin(dlat / 2) + math.cos(math.radians(lat1)) * math.cos(math.radians(lat2)) * math.sin(dlon / 2) * math.sin(dlon / 2)) c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a)) d = radius * c return d
