Skip to content

Instantly share code, notes, and snippets.

@rosterloh
Last active March 13, 2019 12:04
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save rosterloh/9d22ef3bfd804895b68febe6e871eca4 to your computer and use it in GitHub Desktop.
Save rosterloh/9d22ef3bfd804895b68febe6e871eca4 to your computer and use it in GitHub Desktop.
from machine import I2C, Pin
import ssd1306
import mpu6050
from simulation import Simulation
i2c = I2C(scl=Pin(5), sda=Pin(4))
display = ssd1306.SSD1306_I2C(128, 64, i2c)
accel = mpu6050.accel(i2c)
s = Simulation(accel, display)
s.run()
import math
class Point3D:
def __init__(self, x = 0, y = 0, z = 0):
self.x, self.y, self.z = x, y, z
def rotateX(self, deg):
""" Rotates this point around the X axis the given number of degrees. """
rad = deg * math.pi / 180
cosa = math.cos(rad)
sina = math.sin(rad)
y = self.y * cosa - self.z * sina
z = self.y * sina + self.z * cosa
return Point3D(self.x, y, z)
def rotateY(self, deg):
""" Rotates this point around the Y axis the given number of degrees. """
rad = deg * math.pi / 180
cosa = math.cos(rad)
sina = math.sin(rad)
z = self.z * cosa - self.x * sina
x = self.z * sina + self.x * cosa
return Point3D(x, self.y, z)
def rotateZ(self, deg):
""" Rotates this point around the Z axis the given number of degrees. """
rad = deg * math.pi / 180
cosa = math.cos(rad)
sina = math.sin(rad)
x = self.x * cosa - self.y * sina
y = self.x * sina + self.y * cosa
return Point3D(x, y, self.z)
def project(self, win_width, win_height, fov, viewer_distance):
""" Transforms this 3D point to 2D using a perspective projection. """
factor = fov / (viewer_distance + self.z)
x = self.x * factor + win_width / 2
y = -self.y * factor + win_height / 2
return Point3D(x, y, self.z)
from point3d import Point3D
class Simulation:
def __init__(self, accel, display, width=128, height=64, fov=64, distance=4):
self.accel = accel
self.display = display
self.vertices = [
Point3D(-1,1,-1),
Point3D(1,1,-1),
Point3D(1,-1,-1),
Point3D(-1,-1,-1),
Point3D(-1,1,1),
Point3D(1,1,1),
Point3D(1,-1,1),
Point3D(-1,-1,1)
]
# Define the edges, the numbers are indices to the vertices above.
self.edges = [
# Back
(0, 1), (1, 2), (2, 3), (3, 0),
# Front
(5, 4), (4, 7), (7, 6), (6, 5),
# Front-to-back
(0, 4), (1, 5), (2, 6), (3, 7),
]
# Dimensions
self.projection = [width, height, fov, distance]
def get_accel(self, samples=10, calibration=None):
# Setup a dict of measure at 0
result = {}
for _ in range(samples):
v = self.accel.get_values()
for m in v.keys():
# Add on value / samples (to generate an average)
result[m] = result.get(m, 0) + v[m] / samples
if calibration:
# Remove calibration adjustment
for m in calibration.keys():
result[m] -= calibration[m]
return result
def calibrate(self, threshold=50):
print('Calibrating...', end='')
while True:
v1 = self.get_accel(100)
v2 = self.get_accel(100)
if all(abs(v1[m] - v2[m]) < threshold for m in v1.keys()):
print('Done.')
return v1
def to_int(self, *args):
return [int(v) for v in args]
def run(self):
# Starting angle (unrotated in any dimension)
angleX, angleY, angleZ = 0, 0, 0
calibration = calibrate()
while 1:
data = self.get_accel(10, calibration)
angleX = data['AcX'] / 256
angleY = data['AcY'] / 256
t = []
for v in self.vertices:
# Rotate the point around X axis, then around Y axis, and finally around Z axis.
r = v.rotateX(angleX).rotateY(angleY).rotateZ(angleZ)
# Transform the point from 3D to 2D
p = r.project(*self.projection)
# Put the point in the list of transformed vertices
t.append(p)
self.display.fill(0)
for e in self.edges:
self.display.line(*to_int(t[e[0]].x, t[e[0]].y, t[e[1]].x, t[e[1]].y, 1))
self.display.show()
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment