Omvendt vektor beregning

Hej.

Jeg søger en løsning på et regnestykke, som jeg ikke selv kan regne ud.
Jeg er ved at lave en postprocessor til et cam program (til at lave cnc programmer med) hvor jeg gerne ville kunne lave programmer til en 5 akset cnc fræser.
Selve fræsebanerne er ikke noget problem at få rigtigt ud, men det er tilkørslerne til emnet der volder lidt problemer.

For at starte et sted så, prøver jeg at forklare om fræserens opbygning sådan rent fysisk.
Der er i alt 5 akser x,y,z,a og c
X,Y og Z er placeret på den den øverste del af maskinen, hvor Z er spindel aksen (der hvor det skærende værktøj sidder)
For neden er A og C aksen placeret.
A aksen er en vugge der svinger omkring x aksen. På vuggen sidder C aksen, som svinger omkring Z aksen (så længe A aksen står i 0 grader)
Se evt:

http://www.youtube.com/watch?v=RE4GKBKieo4

Princippet er det samme.

Men nu til det endelige spørgsmål:
De koordinater jeg får ud af cam programmet vender altid rigtigt i forhold til emnets x,y og z akse, uanset hvad a og c verdierne hedder.
EKS:
1. A=0 C=0 Hvis jeg bevæger mig fra x0,y0,z0 til x0,y100,z0 flytter maskinen y+ 100mm i forhold til maskinens akser.
2. A=0 C=180 Hvis jeg bevæger mig fra x0,y0,z0 til x0,y100,z0 flytter maskinen y- 100mm i forhold til maskinens akser.

DET jeg så gerne vil kunne regne ud er maskinkoordinaterne (koordinaterne jeg har er jo emne koordinater), som skal bruges ved tilkørsel til emnet.

EKS:
1. A=0 C=0 her er der ingen problemer - emnets akser vender som maskinens akser. x10=x10 y10=y10 z10=z10
2. A=0 C=180 Her kommer problemet så. Hvis emnets kordinater hedder x10,y10,z10 så vil maskinens kordinater være x-10,y-10,z10
3. A=-90 C=0 Her er problemet igen. Hvis emnets kordinater hedder x0,y0,z10 så vil maskinens kordinater være x0,y-10,z0
4. A=-90 C=180 Og igen. Hvis emnets kordinater hedder x10,y10,z10 så vil maskinens kordinater være x-10,y-10,z-10

Det er nogetlunde til at finde ud af i hovedet med ovenstående eksempler, men vil man ikke kunne udregne maskinens x,y og z koordinater ,hvis nu emnets koordinater feks. var A-37,5 C268 x25 y35 z60

For mig virker udregningen ret svær, men det burde vel kunne lade sig gøre?

De tal der er til rådighed er a,c,x,y og z aksernes verdier i forhold til emnets koordinatsystem. Og det jeg skal bruge er maskinens x,y og z koordinater.

Man skal egentlig forestille sig at vektoren, som starter i x0,y0 og z0 står lodret (parallel med z aksen). Det er så ende punktet af den vektor jeg ønsker at finde x,y og z koordinaterne på, hvor a aksens omdrejningspunkt er parallel med x aksen.

Håber at jeg har gjort mig forståelig, eller bare spørg :-)

Forresten så hedder a-aksens verdier +-180 grader og c-aksens verdier 0-360 grader.

Eksemplet overfor A-37,5 C268 x25 y35 z60 skulle gerne give resultatet X-35.851, Y-17.673, Z 62.067

På forhånd tak for hjælpen.

-Stefan