def __pow__(self, other): # cross product
a = self.center()
b = other.center()
return Vector3d(
Point3d(0, 0, 0),
Point3d([
a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2],
a[0] * b[1] - a[1] * b[0]
]))
def __drift_management(self, target):
"""can be called to get closer to target"""
drift = self.position - target
#logging.debug("Drift-Management-before: Actual=%s, Target=%s, Drift=%s(%s)", self.position, target, drift, drift.length())
assert drift.length() < Point3d(1.0, 1.0, 1.0).length()
self.__goto(target)
drift = self.position - target
#logging.debug("Drift-Management-after: Actual=%s, Target=%s, Drift=%s(%s)", self.position, target, drift, drift.length())
assert drift.length() < Point3d(1.0, 1.0, 1.0).length()
def __init__(self, *args):
if len(args) == 0: # default
self.tail = Point3d(0, 0, 0)
self.head = Point3d(1, 1, 1)
elif len(args) == 2:
if type(args[0]) == Point3d and type(args[1]) == Point3d:
self.tail = args[0]
self.head = args[1]
else:
self.tail = Point3d(args[0])
self.head = Point3d(args[1])
else:
raise (TypeError)
def __init__(self, surface, resolution=1.0, default_speed=1.0, delay=0.0):
"""
initialize Controller Object
@param
resolution -> 1.0 means 1 step 1 mm
for our purpose - laser engraver - 256 steps are 36mm so resultion is 36 / 256 = 0,1406
gcode mm to steps = 1 mm = 256/36 =
"""
self.default_speed = default_speed
self.resolution = resolution
self.surface = surface
self.delay = float(delay) / 1000 # in ms
# initialize position
self.position = Point3d(0, 0, 0)
# timemark of last stepping
self.last_step_time = time.time()
# defaults to absolute movements
self.move = self.move_abs
# defaults to millimeter
self.unit = "millimeter"
# motors dict
self.motors = {}
self.spindle = None
self.speed = 0.0
# pygame specificas to draw correct
self.pygame_zoom = 1
self.pygame_draw = True
self.pygame_color = pygame.Color(255, 255, 255, 255)
# define minimal arc step
self.angle_step = math.pi / 180
self.angle_step_sin = math.sin(self.angle_step)
self.angle_step_cos = math.cos(self.angle_step)
def __mul__(self, other): # dot product (or scale)
if type(other) in (int, float):
return Vector3d(
self.tail,
Point3d(tuple(comp * other
for comp in self.center())) + self.tail)
a = self.center()
b = other.center()
return sum(a[i] * b[i] for i in range(3))
def evaluate(self, xValue, yValue, zValue):
if type(self.xSymbol) == int or type(self.xSymbol) == float:
xTemp = self.xSymbol
else:
xTemp = self.xSymbol.evalf(5, subs={x: value})
if type(self.ySymbol) == int or type(self.ySymbol) == float:
yTemp = self.ySymbol
else:
yTemp = self.ySymbol.evalf(5, subs={y: value})
if type(self.zSymbol) == int or type(self.zSymbol) == float:
zTemp = self.zSymbol
else:
zTemp = self.zSymbol.evalf(5, subs={z: value})
h = Point3d(xValue, yValue, zValue)
p = Point3d(xTemp, yTemp, zTemp)
return Vector3d(h, h + p)
def M2(self, *args):
logging.debug("M2 end the program called with %s", args)
# back to origin
self.__goto(Point3d(0, 0, 0))
# unhold everything
for _, motor in self.motors.items():
motor.unhold()
# stop spindle
self.spindle.unhold()
raise StandardError("M02 received, end of program")
def __goto(self, target):
"""
calculate vector between actual position and target position
scale this vector to motor-steps-units und split the
vector in unit vectors with length 1, to control single motor steps
"""
#logging.debug("__goto called with %s", target)
#logging.debug("moving from %s mm to %s mm", self.position, target)
#logging.debug("moving from %s steps to %s steps", self.position * self.resolution, target * self.resolution)
move_vec = target - self.position
if move_vec.length() == 0.0:
#logging.info("move_vec is zero, nothing to draw")
# no movement at all
return
# steps on each axes to move
# scale from mm to steps
move_vec_steps = move_vec * self.resolution
move_vec_steps_unit = move_vec_steps.unit()
#logging.error("move_vec_steps_unit=%s", move_vec_steps_unit)
#logging.error("scaled %s mm to %s steps", move_vec, move_vec_steps)
#logging.error("move_vec_steps.length() = %s", move_vec_steps.length())
# use while loop the get to the exact value
while move_vec_steps.length() > 1.0:
self.__step(move_vec_steps_unit)
#logging.error("actual length left to draw in tiny steps: %f", move_vec_steps.length())
move_vec_steps = move_vec_steps - move_vec_steps_unit
# the last fraction left
self.__step(move_vec_steps)
if self.surface is not None:
self.pygame_update(target)
self.position = target
# after move check controller position with motor positions
motor_position = Point3d(self.motors["X"].get_position(),
self.motors["Y"].get_position(),
self.motors["Z"].get_position())
drift = self.position * self.resolution - motor_position
#logging.debug("Target Drift: Actual=%s; Target=%s; Drift=%s", self.position, target, self.position - target)
#logging.debug("Steps-Drift : Motor=%s; Drift %s length=%s; Spindle: %s", \
# motor_position, drift, drift.length(), self.spindle.get_state())
# drift should not be more than 1 step
# drift could be in any direction 0.999...
assert drift.length() < Point3d(1.0, 1.0, 1.0).length()
def __get_center(self, target, radius):
"""get center from target on circle and radius given"""
logging.info("__get_center called with %s", (target, radius))
distance = target - self.position
# logging.info("x=%s, y=%s, r=%s", x, y, r)
h_x2_div_d = math.sqrt(4 * radius**2 - distance.X**2 -
distance.Y**2) / math.sqrt(distance.X**2 +
distance.Y**2)
i = (distance.X - (distance.Y * h_x2_div_d)) / 2
j = (distance.Y + (distance.X * h_x2_div_d)) / 2
return (Point3d(i, j, 0.0))
def tangentVector(self, value): # find the non-unit tangent vector at the given value
if type(self.xSymbol) == int or type(self.xSymbol) == float:
xTail = self.xSymbol
xHead = 0
else:
xTail = self.xSymbol.evalf(5, subs={t: value})
xHead = diff(self.xSymbol, t).evalf(5, subs={t: value})
if type(self.ySymbol) == int or type(self.ySymbol) == float:
yTail = self.ySymbol
yHead = 0
else:
yTail = self.ySymbol.evalf(5, subs={t: value})
yHead = diff(self.ySymbol, t).evalf(5, subs={t: value})
if type(self.zSymbol) == int or type(self.zSymbol) == float:
zTail = self.zSymbol
zHead = 0
else:
zTail = self.zSymbol.evalf(5, subs={t: value})
zHead = diff(self.zSymbol, t).evalf(5, subs={t: value})
return Vector3d(Point3d(xTail, yTail, zTail), Point3d(xHead+xTail, yHead+yTail, zHead+zTail))
def accelerationVector(self, value):
if type(self.xSymbol) == int or type(self.xSymbol) == float:
xTail = self.xSymbol
xHead = 0
else:
xTail = self.xSymbol.evalf(subs={t: value})
xHead = diff(diff(self.xSymbol, t)).evalf(subs={t: value})
if type(self.ySymbol) == int or type(self.ySymbol) == float:
yTail = self.ySymbol
yHead = 0
else:
yTail = self.ySymbol.evalf(subs={t: value})
yHead = diff(diff(self.ySymbol, t)).evalf(subs={t: value})
if type(self.zSymbol) == int or type(self.zSymbol) == float:
zTail = self.zSymbol
zHead = 0
else:
zTail = self.zSymbol.evalf(subs={t: value})
zHead = diff(diff(self.zSymbol, t)).evalf(subs={t: value})
return Vector3d(Point3d(xTail, yTail, zTail), Point3d(xHead+xTail, yHead+yTail, zHead+zTail))
def normalVector(self, value): # not unit length
dx = diff(self.xSymbol, t)
dy = diff(self.ySymbol, t)
dz = diff(self.zSymbol, t)
tanMagnitude = sqrt(dx**2 + dy**2 + dz**2)
dx2 = diff(dx/tanMagnitude, t)
dy2 = diff(dy/tanMagnitude, t)
dz2 = diff(dz/tanMagnitude, t)
if type(self.xSymbol) == int or type(self.xSymbol) == float:
xTail = self.xSymbol
else:
xTail = self.xSymbol.evalf(subs={t: value})
if type(self.ySymbol) == int or type(self.ySymbol) == float:
yTail = self.ySymbol
else:
yTail = self.ySymbol.evalf(subs={t: value})
if type(self.zSymbol) == int or type(self.zSymbol) == float:
zTail = self.zSymbol
else:
zTail = self.zSymbol.evalf(subs={t: value})
if type(dx2) == int or type(dx2) == float:
xHead = dx2
else:
xHead = dx2.evalf(subs={t: value})
if type(dy2) == int or type(dy2) == float:
yHead = dy2
else:
yHead = dy2.evalf(subs={t: value})
if type(dz2) == int or type(dz2) == float:
zHead = dz2
else:
zHead = dz2.evalf(subs={t: value})
return Vector3d(Point3d(xTail, yTail, zTail), Point3d(xHead+xTail, yHead+yTail, zHead+zTail))
def evaluate(self, value):
if type(self.xSymbol) == int or type(self.xSymbol) == float:
xTemp = self.xSymbol
else:
xTemp = self.xSymbol.evalf(5, subs={t: value})
if type(self.ySymbol) == int or type(self.ySymbol) == float:
yTemp = self.ySymbol
else:
yTemp = self.ySymbol.evalf(5, subs={t: value})
if type(self.zSymbol) == int or type(self.zSymbol) == float:
zTemp = self.zSymbol
else:
zTemp = self.zSymbol.evalf(5, subs={t: value})
return Point3d(xTemp, yTemp, zTemp)
def move_abs(self, *args):
"""
absolute movement to position
args[X,Y,Z] are interpreted as absolute positions
it is not necessary to give alle three axis, when no value is
present, there is not movement on this axis
"""
#logging.info("move_abs called with %s", args)
if args[0] is None:
return
data = args[0]
target = Point3d(0.0, 0.0, 0.0)
for axis in ("X", "Y", "Z"):
if axis in data:
target.__dict__[axis] = data[axis]
else:
target.__dict__[axis] = self.position.__dict__[axis]
#logging.info("target = %s", target)
self.__goto(target)
def move_inc(self, *args):
"""
incremental movement, parameter represents relative position change
move to given x,y ccordinates
x,y are given relative to actual position
so to move in both direction at the same time,
parameter x or y has to be sometime float
"""
#logging.info("move_inc called with %s", args)
if args[0] is None:
return
data = args[0]
target = Point3d(0, 0, 0)
for axis in ("X", "Y", "Z"):
if axis in data:
target.__dict__[
axis] = self.position.__dict__[axis] + data[axis]
else:
target.__dict__[axis] = self.position.__dict__[axis]
#logging.info("target = %s", target)
self.__goto(target)
choice = eval(input())
print()
print()
while(choice == 1):
print("Testing Point3d")
print()
print(" 1. Add 2 points")
print(" 2. Subtract 2 points")
print(" 3. Find the distance between 2 points")
print(" 4. Return to main menu")
subchoice = eval(input())
print()
print()
if subchoice == 1:
coords = getPointsInput()
p1 = Point3d(coords[0])
p2 = Point3d(coords[1])
print(str(p1) + " + " + str(p2) + " = " + str(p1+p2))
print()
elif subchoice == 2:
coords = getPointsInput()
p1 = Point3d(coords[0])
p2 = Point3d(coords[1])
print(str(p1) + " - " + str(p2) + " = " + str(p1-p2))
print()
elif subchoice == 3:
coords = getPointsInput()
p1 = Point3d(coords[0])
p2 = Point3d(coords[1])
print("The distance between " + str(p1) + " and " + str(p2) + " is " + str(p1.distance(p2)))
print()
def __repr__(self):
return "Vector3d object: " + str(self.tail) + " to " + str(self.head)
def __getitem__(self, key):
# Allow numbers to be used to get a point along the vector via the index operator.
# 0 returns the tail, 1 returns the head, and any float between returns a point that far along the vector.
# If key smaller than 0 or larger than 1, the function will return an extrapolated point.
if key == 0: # check to see if we can save performance
return self.tail
if key == 1: # check to see if we can save performance
return self.head
return (self.normalize() * self.magnitude() * key).head
if __name__ == "__main__":
tail = Point3d(5, 2, 6)
head = Point3d(3, 4, 7)
test = Vector3d(tail, head)
test2 = Vector3d()
print("Created " + str(test))
print("The magnitude is " + str(
test.magnitude())) # This should return sqrt(3), or 1.7320508075688772
print("The normalized vector is: " +
str(test.normalize())) # should return a unit vector
print("The normalized vector's magnitude is " +
str(test.normalize().magnitude())
) # should return 1.0, the magnitude of a unit vector
print("Double the original vector is " + str(test * 2))
print("The double vector's magnitude is " + str((test * 2).magnitude()))
print("Test dot product: " + str(test * test2))
print("Index testing:")
def __floordiv__(
self, num
): # Floor division operator is used because of a 3.x Python "feature" that prevents dividing objects by ints
return Vector3d(
self.tail,
Point3d(tuple(comp / num for comp in self.center())) + self.tail)
def __arc(self, *args):
"""
given actual position and
x, y, z relative position of stop point on arc
i, j, k relative position of center
i am not sure if this implementation is straight forward enough
semms more hacked than methematically correct
TODO: Improve
"""
#logging.info("__arc called with %s", args)
#logging.debug("Actual Position at %s", self.position)
data = args[0]
ccw = args[1]
# correct some values if not specified
if "X" not in data: data["X"] = self.position.X
if "Y" not in data: data["Y"] = self.position.Y
if "Z" not in data: data["Z"] = self.position.Z
if "I" not in data: data["I"] = 0.0
if "J" not in data: data["J"] = 0.0
if "K" not in data: data["K"] = 0.0
target = Point3d(data["X"], data["Y"], data["Z"])
#logging.debug("Endpoint of arc at %s", target)
# either R or IJK are given
offset = None
if "R" in data:
offset = self.__get_center(target, data["R"])
else:
offset = Point3d(data["I"], data["J"], data["K"])
#logging.debug("Offset = %s", offset)
center = self.position + offset
#logging.debug("Center of arc at %s", center)
# DELETE radius = offset.length()
#logging.debug("Radius: %s", radius)
# get the angle bewteen the two vectors
target_vec = (target - center).unit()
#logging.debug("target_vec : %s; angle %s", target_vec, target_vec.angle())
position_vec = (self.position - center).unit()
#logging.debug("position_vec : %s; angle %s", position_vec, position_vec.angle())
angle = target_vec.angle_between(position_vec)
#logging.debug("angle between target and position is %s", target_vec.angle_between(position_vec))
start_angle = None
stop_angle = None
angle_step = math.pi / 180
# shortcut, if angle is very small, make a straight line
if abs(angle) <= self.angle_step:
self.__goto(target)
return
if ccw == 1:
# G3 movement
# angle step will be added
# target angle should be greater than position angle
# if not so correct target_angle = 2 * math.pi - target_angle
if target_vec.angle() < position_vec.angle():
start_angle = position_vec.angle()
stop_angle = 2 * math.pi - target_vec.angle()
else:
start_angle = position_vec.angle()
stop_angle = target_vec.angle()
else:
# G2 movement
# so clockwise, step must be negative
# target angle should be smaller than position angle
# if not correct target_angle = 2 * math.pi - target_angle
angle_step = -angle_step
# should go from position to target
if target_vec.angle() > position_vec.angle():
start_angle = position_vec.angle()
stop_angle = 2 * math.pi - target_vec.angle()
else:
start_angle = position_vec.angle()
stop_angle = target_vec.angle()
# this indicates a full circle
if start_angle == stop_angle:
stop_angle += math.pi * 2
angle_steps = abs(int((start_angle - stop_angle) / angle_step))
#logging.debug("Arc from %s rad to %s rad with %s steps in %s radians", start_angle, stop_angle, angle_steps, angle_step)
inv_offset = offset * -1
#logging.debug("Inverse Offset vector : %s", inv_offset)
angle = angle_step * angle_steps
cos_theta = math.cos(angle_step)
sin_theta = math.sin(angle_step)
while abs(angle) > abs(angle_step):
inv_offset = inv_offset.rotated_z_fast(angle_step, cos_theta,
sin_theta)
self.__goto(center + inv_offset)
angle -= angle_step
#logging.debug("angle=%s, start_angle=%s, stop_angle=%s", start_angle + angle, start_angle, stop_angle)
# rotate last tiny fraction left
inv_offset = inv_offset.rotated_Z(angle_step)
self.__goto(center + inv_offset)
# calculate drift of whole arc
arc_drift = self.position - target
#logging.debug("Arc-Drift: Actual=%s, Target=%s, Drift=%s(%s)", self.position, target, arc_drift, arc_drift.length())
assert arc_drift.length() < Point3d(1.0, 1.0, 1.0).length()
self.__drift_management(target)
def __div__(self, num):
return Vector3d(
self.tail,
Point3d(tuple(comp / num for comp in self.center())) + self.tail)
if self.normal.tail[1] < 0: equation += "(y+" + str(abs(self.normal.tail[1])) + ")" elif self.normal.tail[1] > 0: equation += "(y-" + str(self.normal.tail[1]) + ")" else: equation += "y" if n[2] > 0: if equation: equation += " + " elif n[2] < 0: if equation: equation += " - " if n[2] != 0: equation += str(abs(n[2])) if self.normal.tail[2] < 0: equation += "(z+" + str(abs(self.normal.tail[2])) + ")" elif self.normal.tail[2] > 0: equation += "(z-" + str(self.normal.tail[2]) + ")" else: equation += "z" equation += " = 0" return "A plane with the equation " + equation if __name__ == "__main__": test = Plane3d((0, 0, 0), (0, 0, 1), (0, 1, 0)) print(Point3d(0, 69, 420) in test) print(Vector3d() in Plane3d()) print(Plane3d())