def __init__(self):
self.width = WIDTH
self.height = HEIGHT
self.screen = pygame.display.set_mode((WIDTH, HEIGHT))
self.running = True
self.axis_x = Point3d(100, 0, 0)
self.axis_y = Point3d(0, 100, 0)
self.axis_z = Point3d(0, 0, 100)
self.cam_center = Point3d(CENTER)
self.cam_center.y += 100
self.cam_pitch = 0.1
self.cam_yaw = -0.2
self.cam_zoom = 1
self.show_axis = True
self.update = None
def points(self, values):
"""This method parses incoming values being used to define points and
filters them if necessary.
This creates an internal PointList and PointDict, which together mimick
the OrderedDict in Python 3.0. I'm combining these two pieces for backwards
compatibility.
"""
for i, val in enumerate(values):
# Vector3ds will need to be unwrapped
if isinstance(val, Vector3d):
point = Point3d(*val)
else:
# just assume it is some sort of iterable
point = Point3d(*(val[v] for v in range(3)))
# here will build the dictionary, using indices as the only
# value any given tuple refers to
self.pointDict[point] = i
# and here we also build up a list, for lookups by index
self.pointList.append(point)
def __init__(self):
self.view = View()
self.view.cam_center.y += 100
self.base = Point3d(0, 150, 0)
self.arms = [{'t0': 0.0, 't1': 0.0, 't2': 0.0} for i in range(100)]
self.speeds = []
for i in range(100):
s = 0.0005 * i
s = (s if s % 2 == 0 else s * -1)
self.speeds.append(s)
def convert(self, point):
"""Gets a 3d point in the space and converts to a 2d point in the screen"""
# starts by copying the point so we dont change the original
new_point = Point3d(point)
# scales point
new_point.scale(self.cam_zoom)
# applies rotation
new_point.rotateX(self.cam_pitch)
new_point.rotateY(self.cam_yaw)
# inverts y since the screen represents y starting on the top and growing downward
new_point.y *= -1
# translates from the "point world" to the "cam world"
new_point += self.cam_center
# returns a 2d int tuple to be drawn
return (int(new_point.x), int(new_point.y))
def intersect(self, other):
"""Finds the intersection of this plane with another object.
"""
if isinstance(other, Plane3d):
# return the line intersection of two planes
# first, get the cross product of the two plane normals
# which is a vector parallel to L
vector = self.normal.cross(other.normal)
absCoords = [abs(c) for c in vector]
if isRoughlyZero(sum(absCoords)):
# the planes are parallel and do not intersect
return None
else:
# the planes intersect in a line
# first find the largest coordinate in the vector
cNum = None
cMax = 0
for i, c in enumerate(absCoords):
if c > cMax:
cMax = c
cNum = i
dims = ["x", "y", "z"]
biggestDim = dims.pop(cNum)
p = {}
p[biggestDim] = 0
if biggestDim == "x":
p["y"] = (other.d * self.normal.z -
self.d * other.normal.z) / vector.x
p["z"] = (self.d * other.normal.y -
other.d * self.normal.y) / vector.x
elif biggestDim == "y":
p["x"] = (self.d * other.normal.z -
other.d * self.normal.z) / vector.y
p["z"] = (other.d * self.normal.x -
self.d * other.normal.x) / vector.y
else: # biggest dim is z
p["x"] = (other.d * self.normal.y -
self.d * other.normal.y) / vector.z
p["y"] = (self.d * other.normal.x -
other.d * self.normal.x) / vector.z
point = Point3d(**p)
return Line3d(vector, point)
elif isinstance(other, Line3d):
# return the point intersection of a line and a plane
pass
pass
def drawArm(self, arm):
# joint 1 is in the base
self.view.drawPoint(self.base)
A1 = getMatrix(arm['t0'], -pi / 2., 100, 0)
A2 = getMatrix(arm['t1'], 0, 0, 100)
A3 = getMatrix(arm['t2'], 0, 0, 100)
# joint 2
# bringing from frame 1 to frame 0
p1 = Point3d(A1.dot(Point3d().coords))
# bringing from frame 0 to real world
p1 = self.convert(p1)
self.view.drawPoint(p1, color=(200, 0, 0))
self.view.drawLine(self.base, p1)
# joint 3
# bringing from frame 2 to frame 1
p2 = Point3d(A2.dot(Point3d().coords))
# bringing from frame 1 to frame 0
p2 = Point3d(A1.dot(p2.coords))
# bringing from frame 0 to real world
p2 = self.convert(p2)
self.view.drawPoint(p2, color=(0, 0, 200))
self.view.drawLine(p1, p2)
# joint 4
# bringing from frame 3 to frame 2
p3 = Point3d(A3.dot(Point3d().coords))
# bringing from frame 2 to frame 1
p3 = Point3d(A2.dot(p3.coords))
# bringing from frame 1 to frame 0
p3 = Point3d(A1.dot(p3.coords))
# bringing from frame 0 to real world
p3 = self.convert(p3)
self.view.drawPoint(p3, color=(0, 200, 0))
self.view.drawLine(p2, p3)
from point3d import Point3d
WIDTH = 800
HEIGHT = 600
RED = pygame.Color(255, 0, 0)
GREEN = pygame.Color(0, 255, 0)
BLUE = pygame.Color(0, 0, 255)
BACKGROUND = pygame.Color('black')
POINT_COLOR = pygame.Color(200, 200, 200, 255)
LINE_COLOR = pygame.Color(100, 150, 255, 255)
POINT_RADIUS = 3
LINE_WIDTH = 1
ORIGIN = Point3d()
CENTER = Point3d(WIDTH / 2., HEIGHT / 2., 0)
CAM_MOVE_RATE = 0.5
CAM_ROTATE_RATE = 0.01
CAM_ZOOM_RATE = 0.05
TEST_POINTS = [
Point3d(-100, -100, -100),
Point3d(100, -100, -100),
Point3d(100, 100, -100),
Point3d(-100, 100, -100),
Point3d(-100, -100, 100),
Point3d(100, -100, 100),
Point3d(100, 100, 100),
Point3d(-100, 100, 100)