Obrót punktu wokół osi

Witam,
piszę program na pracę inżynierską i utknąłem w martwym punkcie. Chodzi mi o obrót punktu wokół jakiejś prostej w trójwymiarze.
Nie ma problemu ze zrobieniem tego wzdłóż konkretnej osi, chodzi mi jednak o daną prostą lub definiującą ją dwa punkty. Może ktoś posiada kod, który jest w stanie z dwóch punktów P1(x, y, z) i P2(x, y, z) tworzyć linie, względem której obrócę o kąt Alfa punkt P3 (x,y,z). Znalazłem i zmodyfikowałem taki kod:

procedure RotateAtom(var px: Currency; var py: Currency; var pz: Currency; u, v, w, alpha: Currency);
//px, py, pz - obracany punkt w przestrzeni
//u, v, w - parametru wektora
var
  cosT, sinT, a, ppx, ppy, ppz: Currency;
begin
ppx := px;
ppy := py;
ppz := pz;
u := u/sqrt(sqr(u) + sqr(v) + sqr(w));
v := v/sqrt(sqr(u) + sqr(v) + sqr(w));
w := w/sqrt(sqr(u) + sqr(v) + sqr(w));

cosT := cos(alpha*Pi/180);
sinT := sin(alpha*Pi/180);
a := u*ppx + v*ppy + w*ppz;
px   := (a*u + (ppx-a*u)*cosT + (v*ppz-w*ppy)*sinT );
py   := (a*v + (ppy-a*v)*cosT + (w*ppx-u*ppz)*sinT );
pz   := (a*w + (ppz-a*w)*cosT + (u*ppy-v*ppx)*sinT );


end;

Nie mniej kod ten nie chce mi działać odpowiednio (zrobiłem prostą od (0, 0, 0) do (10, 10, 10) oraz punkt nad nią (5, 6, 5), który kręci się w każdą możliwą stronę, tylko nie tak jak trzeba). Bardzo prosiłbym o pomoc w tym problemie, siedze nad tym dwa dni i nie potrafię nic wykombinować.
Pozdrawiam i liczę na pomoc : ))

Moze cos nie tak jest w rozumowaniu danych wejściowych przeze mnie, tj z wektorem? Dla podanego przeze mnie przykładu jak powinien on wyglądac, sam nie jestem w stanie sobie go wyobrazić (tzn jestem ale zawsze wychodzą głupoty..).

Że nikt nic nie pisał to zapytam się czy możesz dać cały program bo ja jakoś nie mogę sobie tego wyobrazić (jak rysujesz kule 3D) w Delphi czy co tam masz. Po prostu nie mogę tego ogarnąć wyobraźnią :]

Zapytam jeszcze czy jesteś pewien co do danych wejściowych (punkt obrotowy).

Macierz obrotu wokół dowolnej osi (dowolnej prostej skierowanej przechodzącej przez początek układu współrzędnych) opisano na angielskiej Wikipedii (http://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_and_angle)

(źródło: en.wikipedia.org)

(u_x, u_y, u_z) - wektor jednostkowy (czyli o długości równej 1) wskazujący kierunek prostej wokół której obracamy
θ - kąt, o który obracamy

Zależność pomiędzy kierunkiem obrotu a kierunkiem osi określa reguła prawej dłoni.

Wektor u można prosto wyznaczyć z punktów P1 i P2 (zakładając kierunek od P1 do P2):
u = (P2 - P1) / |P2 - P1|

Z tym, że jest to obrót wokół prostej przechodzącej przez początek układu współrzędnych (kod który podałeś jest prawdopodobnie właśnie od tego gdyż w danych wejściowych nie ma punktu odniesienia). Jeśli chcemy aby osią obrotu była dowolna prosta przechodząca przez dwa punkty (P1 i P2) można prosto sobie z tym poradzić dokonując translacji (przesunięcia) o -P1 przed obrotem i z powrotem o P1 już po obrocie (zamiast P1 możemy użyć dowolny inny punkt tej prostej np. P2, ale może zostańmy już przy tym P1).

Macierz translacji o -P1 (nazwijmy ją T) ma postać:
1 0 0 -P1x
0 1 0 -P1y
0 0 1 -P1z
0 0 0 1

Macierz translacji o P1 (nazwijmy ją V) ma postać:
1 0 0 P1x
0 1 0 P1y
0 0 1 P1z
0 0 0 1

Musimy jeszcze rozszerzyć macierz obrotu R do rozmiaru 4x4, nową macierz nazwijmy Q:
R11 R12 R13 0
R21 R22 R23 0
R31 R32 R33 0
0 0 0 1

Ostateczna macierz transformacji jaką musisz wyliczyć to V * Q * T