roll/pitch/yaw in turtle

Author: arpruss

build.gradle

@@ -17,7 +17,7 @@ buildscript {
 
 apply plugin: 'forge'
 
-version = "0.12"
+version = "0.13"
 group= "mobi.omegacentauri.raspberryjammod" // http://maven.apache.org/guides/mini/guide-naming-conventions.html
 archivesBaseName = "RaspberryJamMod"
 

python2-scripts.zip

@@ -0,0 +1,37 @@
+from turtle import *
+import random
+
+def tree(depth,thickness,branchLen):
+    if depth <= 0:
+        return
+    if random.random() < 0.2:
+        return
+    if branchLen < 4:
+        t.penblock(LEAVES)
+    else:
+        t.penblock(WOOD)
+    t.penwidth(thickness)
+    t.go(branchLen)
+    newThickness = thickness / 2
+    if newThickness < 1:
+        newThickness = 1
+    newBranchLen = branchLen * 0.75
+    if branchLen < 1:
+        branchLen = 1
+    for i in range(4):
+        t.pitch(30)
+        tree(depth-1,newThickness,newBranchLen)
+        t.pitch(-30)
+        t.roll(90)
+    t.penup()
+    t.back(branchLen)
+    t.pendown()
+
+t = Turtle()
+t.turtle(None)
+t.pendelay(0)
+t.penup()
+t.go(10)
+t.verticalangle(90)
+t.pendown()
+tree(8,8,20)

python2-scripts/mcpipy/fancytree.py

@@ -1,12 +1,15 @@
 import mcpi.minecraft as minecraft
-#import mcpi.block as block
+import mcpi.block as block
 from mcpi.block import *
 from mcpi.entity import *
 import time
 from math import *
 import server
 
 class Turtle:
+    TO_RADIANS = pi / 180.
+    TO_DEGREES = 180. / pi
+
     def __init__(self,mc=None):
         if mc:
              self.mc = mc
@@ -16,7 +19,6 @@ def __init__(self,mc=None):
         self.width = 1
         self.pen = True
         self.directionIn()
-        self.pitch = 0
         self.positionIn()
         self.delayTime = 0.05
         self.nib = []
@@ -66,11 +68,13 @@ def goto(self,x,y,z):
         self.delay()
 
     def verticalangle(self,angle):
-        self.pitch = -angle
+        angles = self.getMinecraftAngles();
+        self.matrix = Turtle.matrixMultiply(Turtle.yawMatrix(angles[0]), Turtle.pitchMatrix(angle))
         self.directionOut()
 
     def angle(self,angle):
-        self.rotation = -angle
+        angles = self.getMinecraftAngles()
+        self.matrix = Turtle.matrixMultiply(Turtle.yawMatrix(angle), Turtle.pitchMatrix(angles[1]))
         self.directionOut()
 
     def penup(self):
@@ -94,25 +98,76 @@ def delay(self):
             time.sleep(self.delayTime)
 
     def directionIn(self):
-        self.rotation = self.mc.player.getRotation()
-        self.pitch = self.mc.player.getPitch()
+        rotation = self.mc.player.getRotation()
+        pitch = 0 #self.mc.player.getPitch()
+        self.matrix = Turtle.matrixMultiply(Turtle.yawMatrix(rotation), Turtle.pitchMatrix(-pitch))
 
-    def directionOut(self):
-        self.pitch %= 360.
-        self.rotation %= 360
+    @staticmethod
+    def matrixMultiply(a,b):
+        c = [[0,0,0],[0,0,0],[0,0,0]]
+        for i in range(3):
+            for j in range(3):
+                c[i][j] = a[i][0]*b[0][j] + a[i][1]*b[1][j] + a[i][2]*b[2][j]
+        return c
 
-        if self.turtleType:
-            pitch = self.pitch
-            rotation = self.rotation
+    @staticmethod
+    def yawMatrix(angleDegrees):
+        theta = angleDegrees * Turtle.TO_RADIANS
+        return [[cos(theta), 0, -sin(theta)],
+                [0,          1, 0],
+                [sin(theta), 0, cos(theta)]]
+
+    @staticmethod
+    def rollMatrix(angleDegrees):
+        theta = angleDegrees * Turtle.TO_RADIANS
+        return [[cos(theta), -sin(theta), 0.],
+                [sin(theta), cos(theta),0.],
+                [0.,          0.,          1.]]
+
+    @staticmethod
+    def pitchMatrix(angleDegrees):
+        theta = angleDegrees * Turtle.TO_RADIANS
+        return [[1,          0,          0],
+                [0, cos(theta),sin(theta)],
+                [0, -sin(theta),cos(theta)]]
+
+    def yaw(self,angleDegrees):
+        self.matrix = Turtle.matrixMultiply(self.matrix, Turtle.yawMatrix(angleDegrees))
+        self.directionOut()
+        self.delay()
 
-            if pitch >= 270:
-                pitch = pitch - 360.
-            elif pitch >= 90:
-                pitch = 180. - pitch
-                rotation = (rotation + 180.) % 360.
+    def roll(self,angleDegrees):
+        self.matrix = Turtle.matrixMultiply(self.matrix, Turtle.rollMatrix(angleDegrees))
+        self.directionOut()
+        self.delay()
 
-            self.mc.entity.setRotation(self.turtleId, rotation)
-            self.mc.entity.setPitch(self.turtleId, pitch)
+    def pitch(self,angleDegrees):
+        self.matrix = Turtle.matrixMultiply(self.matrix, Turtle.pitchMatrix(angleDegrees))
+        self.directionOut()
+        self.delay()
+
+    def getHeading(self):
+        return [self.matrix[0][2],self.matrix[1][2],self.matrix[2][2]]
+
+    def getMinecraftAngles(self):
+        heading = self.getHeading()
+        xz = sqrt(heading[0]*heading[0] + heading[2]*heading[2])
+        if xz >= 1e-9:
+            rotation = atan2(-heading[0], heading[2]) * Turtle.TO_DEGREES
+        else:
+            rotation = 0
+        pitch = atan2(-heading[1], xz) * Turtle.TO_DEGREES
+        return [rotation,pitch]
+
+    def directionOut(self):
+        if self.turtleType:
+            heading = self.getHeading()
+            xz = sqrt(heading[0]*heading[0] + heading[2]*heading[2])
+            if xz >= 1e-9:
+                rotation = atan2(-heading[0], heading[2]) * Turtle.TO_DEGREES
+                self.mc.entity.setRotation(self.turtleId,rotation)
+            pitch = atan2(-heading[1], xz) * Turtle.TO_DEGREES
+            self.mc.entity.setPitch(self.turtleId,pitch)
 
     def pendelay(self, t):
         self.delayTime = t
@@ -121,24 +176,24 @@ def left(self, angle):
         self.right(-angle)
 
     def right(self, angle):
-        self.rotation += angle
+        self.matrix = Turtle.matrixMultiply(Turtle.yawMatrix(angle), self.matrix)
         self.directionOut()
         self.delay()
 
     def up(self, angle):
-        self.pitch -= angle
-        self.directionOut()
-        self.delay()
+        self.pitch(angle)
 
     def down(self, angle):
         self.up(-angle)
 
     def go(self, distance):
-        pitch = self.pitch * pi/180.
-        rot = self.rotation * pi/180.
-        dx = cos(-pitch) * sin(-rot)
-        dy = sin(-pitch)
-        dz = cos(-pitch) * cos(-rot)
+#        pitch = self.pitch * pi/180.
+#        rot = self.rotation * pi/180.
+        # at pitch=0: rot=0 -> [0,0,1], rot=90 -> [-1,0,0]
+#        dx = cos(-pitch) * sin(-rot)
+#        dy = sin(-pitch)
+#        dz = cos(-pitch) * cos(-rot)
+        [dx,dy,dz] = self.getHeading()
         newX = self.position.x + dx * distance
         newY = self.position.y + dy * distance
         newZ = self.position.z + dz * distance
@@ -151,14 +206,15 @@ def go(self, distance):
         self.delay()
 
     def back(self, distance):
-        pitch = self.pitch * pi/180.
-        rot = self.rotation * pi/180.
-        dx = - cos(-pitch) * sin(-rot)
-        dy = - sin(-pitch)
-        dz = - cos(-pitch) * cos(-rot)
-        newX = self.position.x + dx * distance
-        newY = self.position.y + dy * distance
-        newZ = self.position.z + dz * distance
+#        pitch = self.pitch * pi/180.
+#        rot = self.rotation * pi/180.
+#        dx = - cos(-pitch) * sin(-rot)
+#        dy = - sin(-pitch)
+#        dz = - cos(-pitch) * cos(-rot)
+        [dx,dy,dz] = self.getHeading()
+        newX = self.position.x - dx * distance
+        newY = self.position.y - dy * distance
+        newZ = self.position.z - dz * distance
         self.drawLine(self.position.x, self.position.y, self.position.z,
                         newX, newY, newZ)
         self.position.x = newX
@@ -253,13 +309,10 @@ def drawLine(self, x1, y1, z1, x2, y2, z2):
 
 if __name__ == "__main__":
     t = Turtle()
-    t.pendelay(0.01)
-    t.penwidth(1)
-    t.penblock(GLASS)
+    t.pendelay(0.05)
+    t.penblock(GOLD_BLOCK)
     t.turtle(HORSE)
     for i in range(7):
-        print i
-        t.go(100)
-        t.right(180.0-180./7)
-    t.penup()
+        t.go(60)
+        t.up(180-180/7)
     t.turtle(None)

python2-scripts/mcpipy/turtle.py

@@ -0,0 +1,37 @@
+from turtle import *
+import random
+
+def tree(depth,thickness,branchLen):
+    if depth <= 0:
+        return
+    if random.random() < 0.2:
+        return
+    if branchLen < 4:
+        t.penblock(LEAVES)
+    else:
+        t.penblock(WOOD)
+    t.penwidth(thickness)
+    t.go(branchLen)
+    newThickness = thickness / 2
+    if newThickness < 1:
+        newThickness = 1
+    newBranchLen = branchLen * 0.75
+    if branchLen < 1:
+        branchLen = 1
+    for i in range(4):
+        t.pitch(30)
+        tree(depth-1,newThickness,newBranchLen)
+        t.pitch(-30)
+        t.roll(90)
+    t.penup()
+    t.back(branchLen)
+    t.pendown()
+
+t = Turtle()
+t.turtle(None)
+t.pendelay(0)
+t.penup()
+t.go(10)
+t.verticalangle(90)
+t.pendown()
+tree(8,8,20)