小ワザ/ベクトル演算

  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
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100

 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
-
|
|
|
-
|
|
|
!
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 

#define PI 3.14159265    ;円周率π
 
 
;########################################################
;	ベクトル演算モジュール
 
#module
 
;内積
;	result = DotPrd( vecx1, vecy1, vecz1, vecx2, vecy2, vecz2 )
;	vecx1, vecy1, vecz1	:ベクトルA
;	vecx2, vecy2, vecz2	:ベクトルB
;	ベクトルAとベクトルBの内積を計算して結果をresultに返します。
#defcfunc DotPrd double vecx1, double vecy1, double vecz1, double vecx2, double vecy2, double vecz2
    return vecx1*vecx2 + vecy1*vecy2 + vecz1*vecz2
 
 
;外積
;	CrossPrd vecx1, vecy1, vecz1, vecx2, vecy2, vecz2, vecx3, vecy3, vecz3
;	vecx1, vecy1, vecz1	:ベクトルA
;	vecx2, vecy2, vecz2	:ベクトルB
;	vecx3, vecy3, vecz3	:ベクトルC [out]
;	ベクトルAとベクトルBの外積を計算して結果をベクトルCに返します。
;	(ベクトルC) = (ベクトルA)×(ベクトルB)
#deffunc CrossPrd double vecx1, double vecy1, double vecz1, double vecx2, double vecy2, double vecz2, var vecx3, var vecy3, var vecz3
    vecz3 = vecy1*vecz2 - vecz1*vecy2
    vecx3 = vecz1*vecx2 - vecx1*vecz2
    vecy3 = vecx1*vecy2 - vecy1*vecx2
    return
 
 
;ベクトルの絶対値（ベクトルの長さ）
;	result = absv( vecx1, vecy1, vecz1 )
;	vecx1, vecy1, vecz1	:ベクトルA
;	ベクトルAの絶対値を計算して結果をresultに返します。
#defcfunc absv double vecx1, double vecy1, double vecz1
    return sqrt(vecx1 * vecx1 + vecy1 * vecy1 + vecz1 * vecz1)
 
 
;度→ラジアン
;	result = Deg2Rad( th )
;	角度を度数法（°）から弧度法（rad）に変換します。
#defcfunc Deg2Rad double th
    return PI/180.0*th
 
 
;ラジアン→度
;	result = Rad2Deg( th )
;	角度を弧度法（rad）から度数法（°）に変換します。
#defcfunc Rad2Deg double th
    return 180.0/PI*th
 
 
;ベクトルを正規化
;	VecNormalize vecx, vecy, vecz, newvecx, newvecy, newvecz
;	vecx, vecy, vecz	:ベクトルA
;	newvecx, newvecy, newvecz	:ベクトルB [out]
;	ベクトルAを正規化した値をベクトルBに返します。
#deffunc VecNormalize double vecx, double vecy, double vecz, var newvecx, var newvecy, var newvecz
    ln = absv(vecx, vecy, vecz)
    if ln ! 0.0 {
        newvecx = vecx / ln
        newvecy = vecy / ln
        newvecz = vecz / ln
    } else {
        newvecx = 0.0
        newvecy = 0.0
        newvecz = 0.0
    }
    return
 
 
#global
 
;########################################################
;	描画支援モジュール
 
#module
;矢印描画
;	DrwArrow sx, sy, ex, ey
;	sx, sy : 矢印の根元側座標
;	ex, ey : 矢印の先側座標
;	座標sから座標eへ引いた矢印を描画します。
;	ベクトルの大きさにしたがってやじりの大きさも変わります。
#deffunc DrwArrow int sx,int sy,int ex,int ey
    line sx,sy, ex,ey
 
    x = sx - ex
    y = sy - ey
    ax = (x-y)* 10/14 /5+ex
    ay = (y+x)* 10/14 /5+ey
    line ex,ey, ax,ay
 
    ax = (x+y)* 10/14 /5+ex
    ay = (y-x)* 10/14 /5+ey
    line ex,ey, ax,ay
 
return
 
#global

  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

#include "math3d.hsp"
 
;初期化
inv1x = 0
inv1y = 0
inv1z = 0
inv2x = mousex
inv2y = mousey
inv2z = 0
 
cx = ginfo_winx/2
cy = ginfo_winy/2
cz = 0
cx1 = cx+1
cy1 = cy+1
 
*main
    redraw 1 : await 33 : redraw 0 : color 255,255,255 : boxf : color
 
    inv2x = mousex-cx
    inv2y = mousey-cy
    DrwArrow inv1x+cx, inv1y+cy, inv2x+cx, inv2y+cy    ;矢印描画
 
    pos 0, 0
    x = inv2x - inv1x
    y = inv2y - inv1y
    z = inv2z - inv1z
    mes "長さ：" + absv(x, y, z)
 
    VecNormalize x, y, z,ax,ay,az
    mes "ベクトルを正規化：("+ ax +","+ ay +","+ az +")"
 
    goto *main

  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

#include "math3d.hsp"
 
#module
;任意のベクトルを指定したベクトル方向を基準にして分割する。
#deffunc PSDivVec double inv1x, double inv1y, double inv1z,  double inv2x, double inv2y, double inv2z,  var outv3x, var outv3y, var outv3z, var outv4x, var outv4y, var outv4z
;分割元、分割参考、out参考と同じ方向、outのこり
    dot12 = DotPrd( inv1x, inv1y, inv1z, inv2x, inv2y, inv2z) / absv( inv1x, inv1y, inv1z ) / absv( inv2x, inv2y, inv2z )
    lnv1 = absv( inv1x, inv1y, inv1z)
    lnv2 = absv( inv2x, inv2y, inv2z)
        if lnv2=0 : return 1    ;エラー
    lv3 = lnv1 / lnv2 * dot12
    outv3x = lv3 * inv2x
    outv3y = lv3 * inv2y
    outv3z = lv3 * inv2z
    outv4x = inv1x - outv3x
    outv4y = inv1y - outv3y
    outv4z = inv1z - outv3z
    return
#global
 
;初期化
inv1x = 200.0
inv1y = 100.0
inv1z = 0.0
inv2x = 50.0
inv2y = 0.0
inv2z = 0.0
 
outv3x= 0.0
outv3y= 0.0
outv3z= 0.0
outv4x= 0.0
outv4y= 0.0
outv4z= 0.0
 
cx = ginfo_winx/2    ;描画座標
cy = ginfo_winy/2
 
*main
    redraw 1 : await 33 : redraw 0 : color 255,255,255 : boxf : color
    pos 0,0 : mes "マウスのホイールを回してください。"
 
    ;マウスカーソルのホイール値
    th += mousew/120*5
    inv2x = 50.0 * cos(3.1415/180*th)
    inv2y = 50.0 * sin(3.1415/180*th)
 
    ;マウスの位置
    inv1x = double(mousex)-cx
    inv1y = double(mousey)-cy
 
    ;軸を描画
    DrwArrow cx, cy, cx + inv1x, cy + inv1y
    DrwArrow cx, cy, cx + inv2x, cy + inv2y
 
    ;ベクトルを分割
    PSDivVec inv1x, inv1y, inv1z, inv2x, inv2y, inv2z, outv3x, outv3y, outv3z, outv4x, outv4y, outv4z
 
    ;描画
    color 255,0,0
    line cx, cy, cx + outv3x, cy + outv3y
    line cx, cy, cx + outv4x, cy + outv4y
    line mousex, mousey, cx + outv3x, cy + outv3y
    line mousex, mousey, cx + outv4x, cy + outv4y
 
 
    goto *main