小ワザ/多関節

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79

/* 多関節 sample code */
#const  WAIT_TIME    4      // ウェイト[ms]
#const  JOINT_MAX    8      // 関節の数
#const  JOINT_RADIUS 10     // 関節の半径（表示用）
#const  JOINT_DIST   50     // 関節の間隔
#const  THETA_MAX    0.5    // 関節最大旋回角度
#define SPEED_MAX    0.05   // 角速度最大値
 
    PI = atan(1)*4
    ddim jointX, JOINT_MAX
    ddim jointY, JOINT_MAX
    ddim jointT, JOINT_MAX
    repeat JOINT_MAX
        jointX(cnt) = double(JOINT_DIST * cnt)
        jointY(cnt) = 240.0
        jointT(cnt) = 0.0
    loop
 
*main
    gosub *move     // 関節の移動処理
    gosub *draw     // 各種描画処理
    await WAIT_TIME
    goto *main
 
*move
    // 関節角度の計算
    repeat JOINT_MAX - 1, 1
        reverse_cnt = JOINT_MAX - cnt
        radian = 0.0
        repeat reverse_cnt
            radian += jointT(cnt)
        loop
        // この時点でradianには「reverse_cntとreverse_cnt-1の間の線」の角度が代入されている
 
        // 角速度(angular velocity)の計算
        angVelo = atan(mousey - jointY(reverse_cnt), mousex - jointX(reverse_cnt)) - radian
        if angVelo < -PI : angVelo += PI*2
        if angVelo >  PI : angVelo -= PI*2
        if angVelo == 0.0 : break
 
        // これ以上旋回できないので、1つ根っこ側の関節を検討
        if (0.0 < angVelo)&(jointT(reverse_cnt - 1) == THETA_MAX) : continue
        if (angVelo < 0.0)&(jointT(reverse_cnt - 1) ==-THETA_MAX) : continue
 
        jointT(reverse_cnt - 1) = limitf(jointT(reverse_cnt - 1) + limitf(angVelo, -SPEED_MAX, SPEED_MAX), -THETA_MAX, THETA_MAX)
        break
    loop
 
    // 間接位置の決定
    radian = 0.0
    repeat JOINT_MAX - 1
        radian += jointT(cnt)
        jointX(cnt + 1) = jointX(cnt) + cos(radian) * JOINT_DIST
        jointY(cnt + 1) = jointY(cnt) + sin(radian) * JOINT_DIST
    loop
    return
 
*draw
    redraw 0
    color 255, 255, 255 : boxf
    gosub *drawJoint    // 関節の描画（必要ではない）
    gosub *drawLine     // 線の描画
    redraw 1
    return
 
*drawJoint
    color 64, 64, 64
    repeat JOINT_MAX
        circle jointX(cnt) - JOINT_RADIUS, jointY(cnt) - JOINT_RADIUS, jointX(cnt) + JOINT_RADIUS, jointY(cnt) + JOINT_RADIUS, 0
    loop
    return
 
*drawLine
    color 192, 0, 0
    pos jointX, jointY
    repeat JOINT_MAX - 1, 1
        line jointX(cnt), jointY(cnt)
    loop
    return