Last active
March 13, 2019 12:04
-
-
Save rosterloh/9d22ef3bfd804895b68febe6e871eca4 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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() |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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