-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathtranslation.py
37 lines (30 loc) · 984 Bytes
/
translation.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
# -*- coding: utf-8 -*-
from numpy import arccos
import math
def rotation(tup,theta):
X = math.cos(theta)*tup[0]-math.sin(theta)*tup[1]
Y = math.sin(theta)*tup[0]+math.cos(theta)*tup[1]
return(X,Y)
def findangle(vectorlist,imu):
vectorA = vectorlist[0]
vectorB = vectorlist[1]
magA = math.sqrt(vectorA[0]**2+vectorA[1]**2)
magB = math.sqrt(vectorB[0]**2+vectorB[1]**2)
dot = vectorA[0]*vectorB[0] + vectorA[1]*vectorB[1]
Cos = dot/(magA*magB)
theta = arccos(Cos)
return "theta is rad:{} or deg:{}".format(theta,theta*(180/math.pi))
def findcoordinates(Displacement,Vprime,IMU):
#change v' to normal coordinates
V= rotation(Vprime,IMU)
#print(V)
#now add displacement and V together
D = Displacement
U1 = D[0] + V[0]
U2 = D[1] + V[1]
U = (U1,U2)
return U
imu = 3*math.pi/4
displacement = (6,6)
vprime = (-1,1)
print(findcoordinates(displacement,vprime,imu))