C BODY POSITIONS CALCULATED AND DISPLAYED SUBROUTINE GENRAC(SAVST3,ISTP,NEW,DIST) INTEGER SAVST3 DIMENSION VECINC(5,7) COMMON/MOTION/SYMTAB(5,7,20),DIRTAB(9,2) COMMON/LINKIT/ISTRT,LINKS(6,15) C --INITIALIZE-- KN=SAVST3 KO=KN-1 STEPS=ISTP C --FORM VECINC FOR ALL MODULES SAVE HIPS-- DO 10 M=1,6 DO 10 I=1,5 10 VECINC(I,M)=(SYMTAB(I,M,KN)-SYMTAB(I,M,KO))/STEPS C C --DO HIPS-- ROLL=SYMTAB(2,7,KN)-SYMTAB(2,7,KO) IF(ROLL.NE.0.0)GO TO 12 VECINC(4,7)=0.0 GO TO 15 C 12 S=1.0 IF(ROLL.LT.0.0)S=-1.0 DIR=1.0 C --CLOCK WISE-- IF(SYMTAB(3,7,KN).EQ.9.0)DIR=-1.0 IF((S+DIR).EQ.0.0)ROLL=360.0-ABS(ROLL) C --SET FINAL INCR.-- VECINC(4,7)=DIR*ABS(ROLL)/STEPS C C --USE VECINC AND OLD SYMTAB TO CALC. NEW ABS. VECTOR C --ROLL AND FLEXION- 15 CONTINUE DO 100 ICNT=1,ISTP CNT=ICNT C --NORMALIZE ALL RAC MATRICES-- CALL INITL(NEW) C --FORM RAC S FOR LIMBS-- DO 700 M=3,6 C --GET VECTOR COMPONENTS AND ANGLES-- X=SYMTAB(1,M,KO)+CNT*VECINC(1,M) Y=SYMTAB(2,M,KO)+CNT*VECINC(2,M) Z=SYMTAB(3,M,KO)+CNT*VECINC(3,M) ROLL=SYMTAB(4,M,KO)+CNT*VECINC(4,M) FLEX=SYMTAB(5,M,KO)+CNT*VECINC(5,M) C --FORM YAW AND PITCH ANGLES-- AY=ATAN(ABS(Z)/ABS(X))*57.29578 IF(ABS(Z).LT.(.001))AY=0.0 IF(ABS(X).LT.(.001))AY=90.0 IF((ABS(X)+ABS(Z)).LT.(.001))AY=0.0 C AZ=ATAN(ABS(Y)/SQRT(X*X+Z*Z))*57.29578 IF(ABS(Y).LT.(.001))AZ=0.0 IF(ABS(Y).GT.(.98))AZ=90.0 C --GET PROPER SIGN AND MAG.-- SIGN=1.0 IF(Z.GT.0.0)SIGN=-1.0 IF(X.LT.0.0)AY=180.0-AY AY=SIGN*AY SIGN=1.0 IF(Y.LT.0.0)SIGN=-1.0 AZ=SIGN*AZ C --SORT OUT FILMOD NUMBER-- IF(M.EQ.3)N=6 IF(M.EQ.4)N=3 IF(M.EQ.5)N=13 IF(M.EQ.6)N=10 C -- FORM RAC 123 FORMAT(1X,4F7.2) IF(AY.EQ.0.0)GO TO 20 CALL ROTATE(0.0,AY,0.0,N) 20 IF(AZ.EQ.0.0)GO TO 21 CALL ROTATE(0.0,0.0,AZ,N) 21 IF(ROLL.EQ.0.0)GO TO 22 CALL ROTATE(ROLL,0.0,0.0,N) C --FLEXION-- 22 IF(FLEX.EQ.0.0)GO TO23 N=N+1 CALL ROTATE(0.0,0.0,FLEX,N) 23 CONTINUE 700 CONTINUE C ------------------------------- C --DO HEAD AND TORSO-- DO 800 M=1,2 C --GAET VECTOR AND ROLL-- X=SYMTAB(1,M,KO)+CNT*VECINC(1,M) Y=SYMTAB(2,M,KO)+CNT*VECINC(2,M) Z=SYMTAB(3,M,KO)+CNT*VECINC(3,M) ROLL=SYMTAB(4,M,KO)+CNT*VECINC(4,M) C --GET YAW AND PITCH ANGLES-- AX=ATAN(ABS(Z)/ABS(Y))*57.29578 AZ=ATAN(ABS(X)/SQRT(Y*Y+Z*Z))*57.29578 IF(ABS(Y).LT.(.001))AZ=90.0 C --CORRECT FOR SIGN AND MAG.-- IF(Z.LT.0.0)AX=-AX SIGN=1.0 IF(X.GT.0.0)SIGN=-1.0 IF(Y.LT.0.0)AZ=180.0-AZ AZ=AZ*SIGN C --GET FILMOD NUMBERS-- IF(M.EQ.1)N=9 IF(M.EQ.2)N=2 C --FORM RAC MATRICES-- IF(AX.EQ.0.0)GO TO 31 CALL ROTATE(AX,0.0,0.0,N) 31 IF(AZ.EQ.0.0)GO TO 32 CALL ROTATE(0.0,0.0,AZ,N) 32 IF(ROLL.EQ.0.0)GO TO 33 CALL ROTATE(0.0,ROLL,0.0,N) 33 CONTINUE C 800 CONTINUE C --------------------------------- C --BODY ROTATE-- AY=SYMTAB(2,7,KO)+CNT*VECINC(4,7) IF(AY.NE.0.0)CALL ROTATE(0.0,AY,0.0,1) C --REPLACE HANDS AND FEET INTO NORMAL POSE-- CALL ROTATE(90.0,0.0,0.0,5) CALL ROTATE(-90.0,0.0,0.0,8) CALL ROTATE(0.0,0.0,90.0,12) CALL ROTATE(0.0,0.0,90.0,15) C --CONSTRUCT POSITION FTAME-- IFUNC=6 CALL FILMOD(ISTRT) CALL DRAWIT(DIST,NEW,ISTRT,IFUNC) CALL PULSE(IF,ID) C 100 CONTINUE RETURN END