|
| 1 | +#!/usr/bin/env python3 |
| 2 | +# -*- coding: utf-8 -*- |
| 3 | +""" |
| 4 | +Created on Thu Sep 14 11:48:17 2017 |
| 5 | +
|
| 6 | +@author: amit |
| 7 | +""" |
| 8 | +import sympy as sy |
| 9 | +from PotentialsSym import conjugation, phi_p, phi_n, phi_c |
| 10 | + |
| 11 | +# Define the symbolic variables to be used |
| 12 | +u0,u1,u2,v0,v1,v2,x0,x1,x2,y0,y1,y2 = \ |
| 13 | + sy.symbols('u0,u1,u2,v0,v1,v2,x0,x1,x2,y0,y1,y2', real=True) |
| 14 | + |
| 15 | +# Convert rotation vector vi to unit quaternion |
| 16 | +u = sy.sqrt(u0**2 + u1**2 + u2**2) |
| 17 | +p0 = sy.cos( sy.Rational(1,2)*u ) |
| 18 | +p1 = sy.sin( sy.Rational(1,2)*u )*(u0/u) |
| 19 | +p2 = sy.sin( sy.Rational(1,2)*u )*(u1/u) |
| 20 | +p3 = sy.sin( sy.Rational(1,2)*u )*(u2/u) |
| 21 | +p = [p0, p1, p2, p3] |
| 22 | + |
| 23 | +# Convert rotation vector vj to unit quaternion |
| 24 | +v = sy.sqrt(v0**2 + v1**2 + v2**2) |
| 25 | +q0 = sy.cos( sy.Rational(1,2)*v ) |
| 26 | +q1 = sy.sin( sy.Rational(1,2)*v )*(v0/v) |
| 27 | +q2 = sy.sin( sy.Rational(1,2)*v )*(v1/v) |
| 28 | +q3 = sy.sin( sy.Rational(1,2)*v )*(v2/v) |
| 29 | +q = [q0, q1, q2, q3] |
| 30 | + |
| 31 | +qz = [0,0,1] |
| 32 | +ni = conjugation( p, qz ) |
| 33 | +nj = conjugation( q, qz ) |
| 34 | + |
| 35 | +# Make simplifying substitutions |
| 36 | +u,v,u_2,v_2,u_3,v_3,u_4,v_4 = \ |
| 37 | +sy.symbols('u,v,u_2,v_2,u_3,v_3,u_4,v_4') |
| 38 | +sin_alpha_i,cos_alpha_i,sin_alpha_j,cos_alpha_j = \ |
| 39 | + sy.symbols('sin_alpha_i,cos_alpha_i,sin_alpha_j,cos_alpha_j') |
| 40 | + |
| 41 | +subsList = [(u0**2 + u1**2 + u2**2, u_2), \ |
| 42 | + (v0**2 + v1**2 + v2**2, v_2), \ |
| 43 | + (sy.sqrt(u_2), u), (sy.sqrt(v_2), v), \ |
| 44 | + (u**2, u_2),(v**2, v_2),\ |
| 45 | + (u**3, u_3),(v**3, v_3), \ |
| 46 | + (u**4, u_4),(v**4, v_4 ), \ |
| 47 | + (sy.sin(u/2), sin_alpha_i),(sy.cos(u/2), cos_alpha_i), \ |
| 48 | + (sy.sin(v/2), sin_alpha_j),(sy.cos(v/2), cos_alpha_j)] |
| 49 | + |
| 50 | +ni_s =[] |
| 51 | +nj_s = [] |
| 52 | +for i in range( 3 ): |
| 53 | + ni_s.append( ni[i].subs(subsList) ) |
| 54 | + nj_s.append( nj[i].subs(subsList) ) |
| 55 | + |
0 commit comments