64 lines
1.4 KiB
Plaintext
64 lines
1.4 KiB
Plaintext
&ACCESS RV
|
|
&REL 1
|
|
DEFFCT E6POS OffsetByBase(Position:IN, rRelX:IN, rRelY:IN, rRelZ:IN, rRelA:IN, rRelB:IN, rRelC:IN)
|
|
DECL E6POS Position, rPosition
|
|
DECL REAL rRelX, rRelY, rRelZ, rRelA, rRelB, rRelC
|
|
DECL FRAME FrameTrans, FrameRot, PosTrans, PosRot
|
|
|
|
IF VARSTATE("rRelX") <> #INITIALIZED THEN
|
|
rRelX = 0
|
|
ENDIF
|
|
IF VARSTATE("rRelY") <> #INITIALIZED THEN
|
|
rRelY = 0
|
|
ENDIF
|
|
IF VARSTATE("rRelZ") <> #INITIALIZED THEN
|
|
rRelZ = 0
|
|
ENDIF
|
|
IF VARSTATE("rRelA") <> #INITIALIZED THEN
|
|
rRelA = 0
|
|
ENDIF
|
|
IF VARSTATE("rRelB") <> #INITIALIZED THEN
|
|
rRelB = 0
|
|
ENDIF
|
|
IF VARSTATE("rRelC") <> #INITIALIZED THEN
|
|
rRelC = 0
|
|
ENDIF
|
|
|
|
rPosition = Position
|
|
|
|
FrameTrans = $NULLFRAME
|
|
FrameTrans.X = rRelX
|
|
FrameTrans.Y = rRelY
|
|
FrameTrans.Z = rRelZ
|
|
|
|
FrameRot = $NULLFRAME
|
|
FrameRot.A = rRelA
|
|
FrameRot.B = rRelB
|
|
FrameRot.C = rRelC
|
|
|
|
PosTrans = $NULLFRAME
|
|
PosTrans.X = rPosition.X
|
|
PosTrans.y = rPosition.Y
|
|
PosTrans.Z = rPosition.Z
|
|
|
|
PosRot = $NULLFRAME
|
|
PosRot.A = rPosition.A
|
|
PosRot.B = rPosition.B
|
|
PosRot.C = rPosition.C
|
|
|
|
;Positionstransformation durchfuehren
|
|
rPosition = FrameTrans:PosTrans:FrameRot:PosRot
|
|
|
|
;Status & Turn der Originalposition uebernehmen
|
|
|
|
;Werte der externen Achse der Orignalposition
|
|
;uebernehmen
|
|
rPosition.E1 = Position.E1
|
|
rPosition.E2 = Position.E2
|
|
rPosition.E3 = Position.E3
|
|
rPosition.E4 = Position.E4
|
|
rPosition.E5 = Position.E5
|
|
rPosition.E6 = Position.E6
|
|
|
|
RETURN(rPosition)
|
|
ENDFCT |
