今日は素直に動いた。(^^;;;
先週から、何かと仕事が忙しくなってしまって、平日は帰ったらバタンキューって感じなのです。+土曜出勤(ーー;ふぅで、やっと今日、再開。順運動学の高速化の最後の詰めをしていました。ロドリゲスの式の部分をアセンブリ化したのです。いや~、構造体とか、行列とかって、速度をかなり犠牲にしてますね。C言語から機械語を吐き出して眺めると、半分以上が、アドレス計算なんだもの・・・。で、現時点での、ベンチマークは、全23自由度、各関節の重心位置計算込みで、10回繰り返し、111μ秒です。これ以上、アセンブリ化しても、速度は劇的には変わりそうもないので、高速化はここで終了。あとは、2CPU化を行います。・・・また、週末までお預けか?(^^;;;参考にコードを示しま~す。 .export _fk_core2_fk_core2: FMOV.S FR12,@-R15 FMOV.S FR13,@-R15 FMOV.S FR14,@-R15 FMOV.S FR15,@-R15 MOV R4,R5 ADD #116,R5 FMOV.S @R5,FR2 MOVA TOFIX,R0 FMOV.S @R0,FR4 FMUL FR2,FR4 FTRC FR4,FPUL FSCA FPUL,DR12 FMOV FR12,FR14 FNEG FR14 ADD #-4,R5 MOV.L @R5,R6 TST R6,R6 BT X_AXIS MOV R6,R0 CMP/EQ #1,R0 BT Y_AXIS BRA Z_AXIS NOP .align 4 TOFIX: .FDATA.S F'10430.37835X_AXIS: FLDI1 FR0 FLDI0 FR1 FLDI0 FR2 FLDI0 FR3 FLDI0 FR4 FMOV FR13,FR5 FMOV FR12,FR6 FLDI0 FR7 FLDI0 FR8 FMOV FR14,FR9 FMOV FR13,FR10 FLDI0 FR11 BRA MAT_CAL NOP Y_AXIS: FMOV FR13,FR0 FLDI0 FR1 FMOV FR14,FR2 FLDI0 FR3 FLDI0 FR4 FLDI1 FR5 FLDI0 FR6 FLDI0 FR7 FMOV FR12,FR8 FLDI0 FR9 FMOV FR13,FR10 FLDI0 FR11 BRA MAT_CAL NOP Z_AXIS: FMOV FR13,FR0 FMOV FR12,FR1 FLDI0 FR2 FLDI0 FR3 FMOV FR14,FR4 FMOV FR13,FR5 FLDI0 FR6 FLDI0 FR7 FLDI0 FR8 FLDI0 FR9 FLDI1 FR10 FLDI0 FR11 MAT_CAL: MOV R4,R5 MOV R4,R6 ADD #80,R6 ADD #96,R4 STC SR,R0 MOV #60,R1 SHLL2 R1 OR R0,R1 LDC R1,SR FSCHG FTRV XMTRX,FV0 ADD #16,R5 FMOV.D DR2,@-R5 FMOV.D DR0,@-R5 FTRV XMTRX,FV4 ADD #32,R5 FMOV.D DR6,@-R5 FMOV.D DR4,@-R5 FTRV XMTRX,FV8 ADD #32,R5 FMOV.D DR10,@-R5 FMOV.D DR8,@-R5 FMOV.D @R4+,DR12 FMOV.D @R4+,DR14 FTRV XMTRX,FV12 ADD #32,R5 FMOV.D DR14,@-R5 FMOV.D DR12,@-R5 FRCHG FMOV.D @R6+,DR0 FMOV.D @R6+,DR2 FTRV XMTRX,FV0 ADD #32,R5 FMOV.D DR2,@-R5 FMOV.D DR0,@-R5 FSCHG MOV #60,R1 SHLL2 R1 NOT R1,R2 AND R0,R1 STC SR,R0 AND R2,R0 OR R1,R0 LDC R0,SR FMOV.S @R15+,FR15 FMOV.S @R15+,FR14 FMOV.S @R15+,FR13 FMOV.S @R15+,FR12 RTS NOP .END