Prosty algorytm obrotu ciała sztywnego.

Kiedyś już publikowałem algorytm obrotu BS gdzie napisany na podstawie najprostszych działań matematycznych.

https://www.salon24.pl/u/przestrz/826080,uproszczony-algorytm-symulacji-obrotu-bs#comment-13310892

Wynikało to z tego że nie chciałem używać funkcji programu które nie za bardzo rozumiałem, po za tym rozpisanie równań pokazuje też więcej szczegółów a na tym mi bardzo zależało.

Teraz kiedy nabrałem więcej doświadczenia i zaufania postanowiłem przerobić ten algorytm na bardziej profesjonalny. Jest on napisany dla bryły w której zachodzi symetria na trzech osiach głównych i można wpisać dowolne trzy wektory o ile będą one do siebie wzajemnie prostopadłe, oraz dowolną masę tych punktów. Algorytm automatycznie stworzy model dodając symetryczne ramiona. Rachunki są dla trzech punktów (jeden z nich może być zerem) dlatego dla ścisłości wynik dla całej bryły należy pomnożyć przez 2.

import numpy as np
from visual import *
from numpy.linalg import inv

m1=3           #masy na osiach 1-x, 2-y, 3-z
m2=1
m3=0
r1=vector(1,0,0)   #dlugosc ramion
r2=vector(0,1,0)
r3=vector(0,0,1)

W=vector(0.,1.5,0.1)   #omega startow

bryla=frame()    #definowanie bryly sztywnej
if mag(r1)>0:
    masa11=sphere(frame=bryla, pos = vector(r1.x,r1.y,r1.z), radius =0.02, color = color.red)
    masa12=sphere(frame=bryla, pos = vector(-r1.x,-r1.y,-r1.z), radius =0.02, color = color.red)
    ramie1=cylinder(frame=bryla, pos = masa11.pos, radius =0.005, axis=masa12.pos-masa11.pos)
if mag(r2)>0:
    masa21=sphere(frame=bryla, pos = vector(r2.x,r2.y,r2.z), radius =0.02, color = color.blue)
    masa22=sphere(frame=bryla, pos = vector(-r2.x,-r2.y,-r2.z), radius =0.02, color = color.blue)
    ramie2=cylinder(frame=bryla, pos = masa21.pos, radius =0.005, axis=masa22.pos-masa21.pos)
if mag(r3)>0:
    masa31=sphere(frame=bryla, pos = vector(r3.x,r3.y,r3.z), radius =0.02, color = color.green)
    masa32=sphere(frame=bryla, pos = vector(-r3.x,-r3.y,-r3.z), radius =0.02, color = color.green)
    ramie3=cylinder(frame=bryla, pos = masa31.pos, radius =0.005, axis=masa32.pos-masa31.pos)

oX=arrow(axis=vector(1,0,0), shaftwidth=0.01, color =vector(0.7,0.7,0))     #osie ukladu inercjalnego
oY=arrow(axis=vector(0,1,0), shaftwidth=0.01, color =vector(0.7,0.7,0))
oZ=arrow(axis=vector(0,0,1), shaftwidth=0.01, color =vector(0.7,0.7,0))

dt=0.001
t=0

while t<1000:
    rate(200)
    #tensro momentu bezwladnosci I
    I= np.array([[m1*(r1.y*r1.y+r1.z*r1.z)+m2*(r2.y*r2.y+r2.z*r2.z)+m3*(r3.y*r3.y+r3.z*r3.z), -m1*r1.x*r1.y-m2*r2.x*r2.y-m3*r3.x*r3.y, -m1*r1.x*r1.z-m2*r2.x*r2.z-m3*r3.x*r3.z],
             [-m1*r1.y*r1.x-m2*r2.y*r2.x-m3*r3.y*r3.x, m1*(r1.x*r1.x+r1.z*r1.z)+m2*(r2.x*r2.x+r2.z*r2.z)+m3*(r3.x*r3.x+r3.z*r3.z), -m1*r1.y*r1.z-m2*r2.y*r2.z-m3*r3.y*r3.z],
             [-m1*r1.z*r1.x-m2*r2.z*r2.x-m3*r3.z*r3.x, -m1*r1.z*r1.y-m2*r2.z*r2.y-m3*r3.z*r3.y, m1*(r1.y*r1.y+r1.x*r1.x)+m2*(r2.y*r2.y+r2.x*r2.x)+m3*(r3.y*r3.y+r3.x*r3.x)]])
    L=I.dot(W)                  #moment pedu L=IW
    M=cross(L,W)                #moment sily M=LxW
    OI=inv(I)                   #odwrotnosc tensora momentu bezladnosci I^-1
    e=OI.dot(M)                 #przyspieszenie katowe e=MI^-1
    E=vector(e[0],e[1],e[2])    #transformacja macierzy na wektor
    if t==0:
        print "L", mag(L),L,"M",M,"e",e

    E=E.rotate(angle=mag(W)*dt,axis=W)      #obrot wektora przyspieszenia katowego
    bryla.rotate (angle=mag(W)*dt,axis=W)   #obrot bryly sztywnej

    W=W+(E*dt)                  #przyspieszenie katowe

    r1=bryla.frame_to_world(masa11.pos)     #wyznaczanie nowych wspolrzednych punktow
    r2=bryla.frame_to_world(masa21.pos)
    r3=bryla.frame_to_world(masa31.pos)

    t=t+1
print "L",mag(L),L,"M",M,"e",e

7 linijek definiuje pozycje punktów i ich masy oraz prędkość kątową.

16 linijek definiuje wizualizacje modelu

17 linijek stanowi pętle algorytmu

Opis pętli to:

-definicja tensora momentu bezwładności

-wyliczenie przyspieszenia kątowego e=MI^-1=(LxW)I^-1=(IWxW)I^-1

-obrót bryły z prędkością kątową W wraz z wektorem przyspieszenia kątowego

-uaktualnienie prędkości kątowej o przyspieszenie kątowe

-wyznaczenie nowych współrzędnych punktów po obrocie