fc2ブログ

2021年のホビー活動まとめ


2021年のホビーの活動まとめです。



Arduino倒立振子。
途中までやって挫折!
https://note.com/ninagawa123/n/naebba2f4ac7d

これは時間を見つけて来年リベンジ。



Meridian計画。
こちらはなかなかの進捗で、ボードの頒布まで辿り着きました。
まとめはこちら。
https://note.com/ninagawa123/n/nb768563591be



Qiita活動。
こちらも環境構築やら細かいメモを中心にいろいろ書きました。
https://qiita.com/Ninagawa_Izumi



レトロ活動
ロードランナー初代FC クリア
SDF クリア
SEKIRO ラスボス残し



Youtube活動
ROBLOX studioのチュートリアル入門動画をテスト的にやってみました。
第二弾のアドベンチャー編を投稿しました。



オフライン練習会。
今年もDiscordで何度かトークイベントを実施しました。



活動の記録もnoteやqiita、youtubeになってきましたが、
来年もまたよろしくお願いします。
スポンサーサイト



URDF


近況報告がQiitaとTwitter、Qiitadon、noteになってしまってこちらに書き込む機会がすっかり減ってしまいました。。。
自分用のメモもNotionに保存したりしています。



先日はじめてgithubを公開しました。
URDFファイルです。KHRを概念化したようなロボットです。

https://github.com/Ninagawa123/roid1_urdf

URDFの作成についての基本事項もまとめました。
https://qiita.com/Ninagawa_Izumi/items/d65e92877246e649cca6

逆運動学を思い出せない


忘れそうなのでスクリプトをコピペ。
前に頑張って作ったけど意図通りに動いたところで喜んでしまって、メモを残さず全部忘れたので復習が必要。
一旦KHRのシンプルな軸にして作り直す予定。その際にqiita化しておくと確実かもしれない。



--Nurio9のParaServer--
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using System;

public class ParamServer : MonoBehaviour
{
//計算用レジスタの宣言
public float regi1;
public float regi2;
public float regi3;
public float regi4;
public float regi5;
public float regi6;
public float regi7;
public float regi8;

public double housen_x;
public Vector3 housen_y;
public Vector3 housen_z;
public Vector3 housen;//法線のVector3
//public Vector3 housen2;//法線のVector3
//public Vector3 housen3;//法線のVector3
public Vector3 point1_senbun_pointA;//法線のVector3
public Vector3 point1_senbun_pointB;//法線のVector3
public float naisekiA;
public float naisekiB;
public float naiseki_kekka;

public float dr;//回転行列計算用
public float dp;
public float dy;

public double dr2;//原点の回転座標
public double dp2;
public double dy2;

public float[] ServoAnglesL = new float[20];//サーボの角度配列
public float[] ServoAnglesR = new float[20];

public float[] BodyCenter_pos = new float[3] { 0, 0, 0.28870f }; //腰のセンターの値(調整はあとから)
public float[] Point1_pos = new float[3] { 0, 0.05629f, 0.02370f };//赤ポイント、左足首
public float[] Point1_pos_r = new float[3] { 0, -0.05629f, 0.02370f };//赤ポイント、右足首


public float[] Point2_pos = new float[3] { 0, 0, 0 };//左股関節のグローバル座標
public float[] Point3_pos = new float[3] { 0, 0, 0 };//股関節のセンター(平行移動の中心点)
public float[] Point4_pos = new float[3] { 0, 0, 0 };//ローカル座標から計算したときの足裏の位置
public float[] Point5_pos = new float[3] { 0, 0, 0 };//関数判定用の適当な位置
public float[] Point6_pos = new float[3] { 0, 0, 0 };//関数判定用の適当な位置
public float[] Point7_pos = new float[3] { 0, 0, 0 };//関数判定用の回転中心

public float[] Point2_pos_r = new float[3] { 0, 0, 0 };//右足用
public float[] Point3_pos_r = new float[3] { 0, 0, 0 };
public float[] Point4_pos_r = new float[3] { 0, 0, 0 };
public float[] Point5_pos_r = new float[3] { 0, 0, 0 };
public float[] Point6_pos_r = new float[3] { 0, 0, 0 };
public float[] Point7_pos_r = new float[3] { 0, 0, 0 };


public Vector3 point_rocal_pos;//3D移動後の股関節を原点とし、そこから見た目標座標(足裏)のローカル座標
public Vector3 point_grobal_pos;//上記をグローバルに戻す際の変数
public Vector3 point_c2l_pos;//股関節座標の回転計算用ベクトル

public Vector3 point_rocal_pos_r;//右足用
public Vector3 point_grobal_pos_r;
public Vector3 point_c2l_pos_r;


//public float LarmLength = 0.30610f;

public float ureg = 0.1325f;//足の大腿の長さ
public float lreg = 0.1325f;//足の下腿の長さ
public float kokanoff = 0.05629f;//股関節の長さ
public float geta = 0.02370f;//足首から足裏までの長さ(下駄)

public float alpha;//股関節ロール角度[ID10]
public float alpha2;//股関節ロール角度[ID10]
public float beta;//股関節ピッチ角度
public float beta2;//股関節ピッチ角度(前フレーム保持)
public float gamma;//股関節ヨー角度
public float gamma2;//股関節ヨー角度(前フレーム保持)

public float alpha_r;//右足用
public float alpha2_r;
public float beta_r;
public float beta2_r;
public float gamma_r;
public float gamma2_r;


//public double delta2 = 0;//腰ロール角度(前フレーム保持)

//左足の股関節の値(開始位置(可変))
public float x0;
public float y0;
public float z0;

public float x0_r;//右足用
public float y0_r;
public float z0_r;

//左足の股関節の値(開始位置(計算用))
public double x0a;
public double y0a;
public double z0a;

public double x0a_r;//右足用
public double y0a_r;
public double z0a_r;

//腰センター(計算用)
public double x0_center;
public double y0_center;
public double z0_center;

//腰センターを原点としたときの股関節の位置(計算用)
public float x0_c2l;// center to location
public float y0_c2l;
public float z0_c2l;

public float x0_c2l_r;//右足用
public float y0_c2l_r;
public float z0_c2l_r;


//足の足首の初期値(終点位置(固定))
public float x1;
public float y1;
public float z1;

public float x1_r;//右足用
public float y1_r;
public float z1_r;

//足の足首の初期値(終点位置(計算用))
public float x1b;
public float y1b;
public float z1b;

public float x1b_r;//右足用
public float y1b_r;
public float z1b_r;

//足の足首の初期値(終点位置(ローカル座標))
public float x1_loc;
public float y1_loc;
public float z1_loc;

public float x1_loc_r;//右足用
public float y1_loc_r;
public float z1_loc_r;

//計算用
public float x1_loc2;//距離
public float y1_loc2;
public float z1_loc2;


//足の足首の変化後の値(開始位置)
public float x2;
public float y2;
public float z2;

public float x2_r;//右足用
public float y2_r;
public float z2_r;

public float a;
public float b;
public float c;

public float a_r;//右足用
public float b_r;
public float c_r;

//public double abx2;

public double a2;
public double b2;
public double c2;
public double d2;

public double a2_r;//右足用
public double b2_r;
public double c2_r;
public double d2_r;

//public float open_value = 0; //publicな変数を設定
public bool fbSwitch = false; //publicな変数を設定

private void Start()
{
for (int i = 0; i < 20; i++)
{
ServoAnglesL[i] = 0;//各サーボの初期値を0に設定
ServoAnglesR[i] = 0;

//足の足首の初期値(終点位置(固定))
x1 = 0;
y1 = kokanoff;
z1 = geta;

//足の足首の初期値(終点位置(計算用))
x1b = 0;
y1b = kokanoff;
z1b = geta;
}
}


private void Update()
{

/// Point1_pos : 足首の固定位置
/// Point2_pos : 左股関節のグローバル座標
/// Point3_pos : 股関節のセンター,Bodyの中心点
/// Point4_pos : ローカル座標から計算したときの目標点の位置
/// Point5_pos : 股関節ローカル平面と足首x水平線との交点
/// BodyCenter_pos: 腰のセンターの値


/// ▼▼▼ まず股関節(始点位置)のグローバル座標を計算 ▼▼▼
//腰センターの中心点を発行(Point3) 操作による平行移動も反映
Point3_pos[0] = BodyCenter_pos[0];
Point3_pos[1] = BodyCenter_pos[1];
Point3_pos[2] = BodyCenter_pos[2];
//腰センターを原点としたときの股関節の位置(回転前・計算用)
x0_c2l = 0;
y0_c2l = kokanoff;
z0_c2l = 0;
//腰ロール軸のロール(ピッチ)ヨーの回転角を代入
dr = ServoAnglesL[8] * Mathf.Deg2Rad;//ロール角度 ラジアン
dp = ServoAnglesL[7] * Mathf.Deg2Rad;//ピッチ角度 ラジアン(使わず)
dy = ServoAnglesL[6] * Mathf.Deg2Rad;//ヨー角度  ラジアン
//腰センターを原点としたときの股関節の回転後の位置を計算
point_c2l_pos = RotMat(x0_c2l, y0_c2l, z0_c2l, dr, dp, dy);
//回転後の位置point_c2l_posに対し、並行移動分のBodyCenter_posを追加し、グローバル座標上の股関節位置(x0,y0,z0)を決定
x0 = point_c2l_pos.x + BodyCenter_pos[0];
y0 = point_c2l_pos.y + BodyCenter_pos[1];
z0 = point_c2l_pos.z + BodyCenter_pos[2];
//股関節ポイント座標(Point2)の発行
Point2_pos[0] = x0;
Point2_pos[1] = y0;
Point2_pos[2] = z0;
// ▲▲▲ 股関節(始点位置)のグローバル座標が定まった。完成 ▲▲▲

// ▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎ 同上右足
x0_c2l_r = 0;
y0_c2l_r = -kokanoff;
z0_c2l_r = 0;

//腰センターを原点としたときの股関節の回転後の位置を計算
point_c2l_pos_r = RotMat(x0_c2l_r, y0_c2l_r, z0_c2l_r, dr, dp, dy);
//回転後の位置point_c2l_posに対し、並行移動分のBodyCenter_posを追加し、グローバル座標上の股関節位置(x0,y0,z0)を決定
x0_r = point_c2l_pos_r.x + BodyCenter_pos[0];
y0_r = point_c2l_pos_r.y + BodyCenter_pos[1];
z0_r = point_c2l_pos_r.z + BodyCenter_pos[2];
//股関節ポイント座標(Point2)の発行
Point2_pos_r[0] = x0_r;
Point2_pos_r[1] = y0_r;
Point2_pos_r[2] = z0_r;
// ▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎ 右足ここまで



/// ▼▼▼ 股関節の法線ベクトルの算出 ▼▼▼
housen = RotMat(1f, 0f, 0f, dr, dp, dy);
housen_z = RotMat(0f, 0f, 1f, dr, dp, dy);
housen_y = RotMat(0f, 1f, 0f, dr, dp, dy);
/// ▲▲▲ 股関節の法線ベクトルグローバル座標が定まった。完成 ▲▲▲


/// ▼▼▼ 股関節ローカルから見た足の位置(Point4)の計算 ▼▼▼
//股関節点(ローカル座標原点かつグローバル並行座標)から見た目標位置x2,y2,z2を設定(目標位置Point1-股関節位置Point2)
x2 = Point1_pos[0] - x0;
y2 = Point1_pos[1] - y0;
z2 = Point1_pos[2] - z0;
//目標点のローカル座標を設定(回転補正前)
x1_loc = x2;
y1_loc = y2;
z1_loc = z2;
//目標点のローカル座標を計算(回転補正後)point_rocal_pos.x(y,z)が目指すべき目標!以下でPoint1_pos[0](1,2)の代わりに使うといいはず!?
point_rocal_pos = RotMat_rev(x1_loc, y1_loc, z1_loc, dr, dp, dy);
// ▲▲▲ 回転後の目標点ローカル座標が定まった。完成 ▲▲▲

// ▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎ 同上右足
//股関節点(ローカル座標原点かつグローバル並行座標)から見た目標位置x2,y2,z2を設定(目標位置Point1-股関節位置Point2)
x2_r = Point1_pos_r[0] - x0_r;
y2_r = Point1_pos_r[1] - y0_r;
z2_r = Point1_pos_r[2] - z0_r;
//目標点のローカル座標を設定(回転補正前)
x1_loc_r = x2_r;
y1_loc_r = y2_r;
z1_loc_r = z2_r;
//目標点のローカル座標を計算(回転補正後)point_rocal_pos.x(y,z)が目指すべき目標!以下でPoint1_pos[0](1,2)の代わりに使うといいはず!?
point_rocal_pos_r = RotMat_rev(x1_loc_r, y1_loc_r, z1_loc_r, dr, dp, dy);
// ▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎ 右足ここまで



// ▼▼▼ ローカルxz平面と目標点からのグローバルx軸平行線との交点をPoint5としたい ▼▼▼
//目標点の線分の長さは仮に前後1とする。
point1_senbun_pointA.x = +1;//目標ポイントのグローバルyz垂線A
point1_senbun_pointA.y = 0.05629f;
point1_senbun_pointA.z = 0.02370f;
point1_senbun_pointB.x = -1;//目標ポイントのグローバルyz垂線B
point1_senbun_pointB.y = 0.05629f;
point1_senbun_pointB.z = 0.02370f;
//内積する http://www.sousakuba.com/Programming/gs_plane_line_intersect.html
naisekiA = Mathf.Abs(Vector3.Dot(housen, point1_senbun_pointA));
naisekiB = Mathf.Abs(Vector3.Dot(housen, point1_senbun_pointB));
//内積の結果より、naisekiAとnaisekiBの比率により、naisekiA点から交点位置を決定。
naiseki_kekka = 1f + (float)((-2f) * (naisekiA / (naisekiA + naisekiB)));
//計算値をPoint5のポジションに反映。Point5のはローカルyz平面と足首ポイントx平行線との交点
Point5_pos[0] = BodyCenter_pos[0] + naiseki_kekka;
Point5_pos[1] = Point1_pos[1];
Point5_pos[2] = Point1_pos[2];
// ▲▲▲ 股関節平面と目標点グローバル平行線との交点(Point5)が決まった。完成 ▲▲▲

// ▼▼▼ Point6はローカル座標を標準座標とみなしてグローバル座標に投影したときの相対的な目標点の位置 ▼▼▼
Point6_pos[0] = point_rocal_pos.x;
Point6_pos[1] = point_rocal_pos.y;
Point6_pos[2] = point_rocal_pos.z;
// ▲▲▲ ローカル座標を標準座標とみなしてグローバル座標に投影したときの相対的な目標点の位置(Point6)が決まった。完成 ▲▲▲


// ▼▼▼ Point5は上記Point6をグローバル座標に戻したもの ▼▼▼
//ローカル座標の目標位置をグローバル座標側に逆回転させ、股関節の原点位置を足す。この点が目標位置点とぴったり合えばよい。
point_grobal_pos = RotMat(point_rocal_pos.x, point_rocal_pos.y, point_rocal_pos.z, dr, dp, dy);//計算したものを一旦目標点のグローバル座標に戻す
Point4_pos[0] = point_grobal_pos.x + x0;
Point4_pos[1] = point_grobal_pos.y + y0;
Point4_pos[2] = point_grobal_pos.z + z0;
// ▲▲▲ Point5が完成。目標点と常にぴったり合う。  ▲▲▲


/// ▼▼▼ 法線を利用して、股関節のヨー軸、ロール軸、ピッチ軸を一旦グローバルと並行に保つ。 ▼▼▼
ServoAnglesL[9] = Mathf.Atan2(housen.y, housen.x) * 180 / Mathf.PI;//■股関節ヨー軸新規計算 //ここで正面を向くことに成功しているはず★
ServoAnglesL[10] = Mathf.Atan2(housen_z.y, housen_z.z) * 180 / Mathf.PI;// ■股関節ロール軸
ServoAnglesL[11] = Mathf.Atan2(housen_z.z, housen_z.x) * 180 / Mathf.PI - 90f;// ■股関節ピッチ軸
// ▲▲▲ 腰のひねりに追従して、股関節面をグローバルにキープすることができた!  ▲▲▲

// ▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎ 同上右足
ServoAnglesR[9] = Mathf.Atan2(housen.y, housen.x) * 180 / Mathf.PI;//■股関節ヨー軸新規計算 //ここで正面を向くことに成功しているはず★
ServoAnglesR[10] = Mathf.Atan2(housen_z.y, housen_z.z) * 180 / Mathf.PI;// ■股関節ロール軸
ServoAnglesR[11] = Mathf.Atan2(housen_z.z, housen_z.x) * 180 / Mathf.PI - 90f;// ■股関節ピッチ軸
// ▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎ 右足ここまで

/// ★★★★★★★★★ 完成しているのでここまではなるべく手をつけない。

/// ▼▼▼ 股関節ロール軸の差分を求める。いったんグローバル座標と並行しているので、そこからの差分だけを素直に計算し、追加。 ▼▼▼
alpha = Mathf.Atan2(y2, z2) * 180 / Mathf.PI;
ServoAnglesL[10] = ServoAnglesL[10] - alpha +180;
// ▲▲▲ 股関節ロール軸を決定することができた!  ▲▲▲

// ▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎ 同上右足
alpha_r = Mathf.Atan2(y2_r, z2_r) * 180 / Mathf.PI;
ServoAnglesR[10] = ServoAnglesR[10] - alpha_r + 180;
// ▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎ 右足ここまで



/// ▼▼▼ 股関節ピッチ軸の差分を求める。いったんグローバル座標と並行しているので、そこからの差分だけを素直に計算し、追加。 ▼▼▼
//股関節位置から目標点までの距離(ローカル座標目標位置を利用)
a = Mathf.Sqrt(Mathf.Pow(y2, 2) + Mathf.Pow(z2, 2));
b = Mathf.Sqrt(Mathf.Pow(x2, 2) + Mathf.Pow(a, 2));

regi1 = Mathf.Atan(a / x2);
regi2 = Mathf.Pow(ureg, 2) - Mathf.Pow(lreg, 2) + Mathf.Pow(x2, 2) + Mathf.Pow(a, 2);
regi3 = 2 * ureg * Mathf.Sqrt(Mathf.Pow(x2, 2) + Mathf.Pow(a, 2));
regi4 = regi2 / regi3;

if (regi2 > regi3) { beta = beta2; }
else
{
if (regi1 < 0)
{
beta = 90f + ((regi1 - Mathf.Acos(regi4)) * 180f / Mathf.PI);
}
else
{
beta = -90f + ((regi1 - Mathf.Acos(regi4)) * 180f / Mathf.PI);
}
}

ServoAnglesL[11] = ServoAnglesL[11] + beta;// Mathf.Atan2(housen_z.z, housen_z.x) * 180 / Mathf.PI - 90f;// ■股関節ピッチ軸

beta2 = beta;

// ▲▲▲ 股関節ピッチ軸を決定することができた!?▲▲▲


// ▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎ 同上右足
//股関節位置から目標点までの距離(ローカル座標目標位置を利用)
a_r = Mathf.Sqrt(Mathf.Pow(y2_r, 2) + Mathf.Pow(z2_r, 2));
b_r = Mathf.Sqrt(Mathf.Pow(x2_r, 2) + Mathf.Pow(a_r, 2));

regi1 = Mathf.Atan(a_r / x2_r);
regi2 = Mathf.Pow(ureg, 2) - Mathf.Pow(lreg, 2) + Mathf.Pow(x2_r, 2) + Mathf.Pow(a_r, 2);
regi3 = 2 * ureg * Mathf.Sqrt(Mathf.Pow(x2_r, 2) + Mathf.Pow(a_r, 2));
regi4 = regi2 / regi3;

if (regi2 > regi3) { beta_r = beta2_r; }
else
{
if (regi1 < 0)
{
beta_r = 90f + ((regi1 - Mathf.Acos(regi4)) * 180f / Mathf.PI);
}
else
{
beta_r = -90f + ((regi1 - Mathf.Acos(regi4)) * 180f / Mathf.PI);
}
}
ServoAnglesR[11] = ServoAnglesR[11] + beta_r;// Mathf.Atan2(housen_z.z, housen_z.x) * 180 / Mathf.PI - 90f;// ■股関節ピッチ軸
beta2_r = beta_r;
// ▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎ 右足ここまで



/// ▼▼▼ 膝関節ピッチ軸 ▼▼▼
regi7 = Mathf.Pow(ureg, 2) + Mathf.Pow(lreg, 2) - Mathf.Pow(x2, 2) - Mathf.Pow(a, 2);
regi8 = (2 * ureg * lreg);

if (Mathf.Abs(regi7) > Mathf.Abs(regi8)) {
gamma = gamma2;
}
else
{
gamma = (Mathf.PI - Mathf.Acos(regi7 / regi8)) * 180f / Mathf.PI;
}

ServoAnglesL[12] = gamma; // ■ひざ関節ピッチ
// ▲▲▲ 膝関節ピッチ軸を決定することができた!?▲▲▲


// ▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎ 同上右足
regi7 = Mathf.Pow(ureg, 2) + Mathf.Pow(lreg, 2) - Mathf.Pow(x2_r, 2) - Mathf.Pow(a_r, 2);
regi8 = (2 * ureg * lreg);

if (Mathf.Abs(regi7) > Mathf.Abs(regi8))
{
gamma_r = gamma2_r;
}
else
{
gamma_r = (Mathf.PI - Mathf.Acos(regi7 / regi8)) * 180f / Mathf.PI;
}

ServoAnglesR[12] = gamma_r; // ■ひざ関節ピッチ
// ▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎ 右足ここまで


/// ▼▼▼足首ピッチ軸 ▼▼▼
ServoAnglesL[13] = - beta - gamma ;// ■足首ピッチ
gamma2 = gamma;
// ▲▲▲ 足首ピッチ軸を決定することができた!?▲▲▲

// ▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎ 同上右足
ServoAnglesR[13] = -beta_r - gamma_r;// ■足首ピッチ
gamma2_r = gamma_r;
// ▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎ 右足ここまで

/// ▼▼▼ 足首ロール軸は 股関節法線補正と股関節距離から角度を算出? ▼▼▼
ServoAnglesL[14] = alpha+180f;
// ▲▲▲ 足首ロール軸を決定することができた!?▲▲▲

// ▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎ 同上右足
ServoAnglesR[14] = alpha_r + 180f;
// ▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎▶︎ 右足ここまで


}












static Vector3 RotMat(float x,float y,float z, float dr, float dp,float dy)//xyzは位置、dr,dp,dyは角度(ラジアン)
{
double x0 = x * (Math.Cos(dy) * Math.Cos(dp)) + y * (-Math.Sin(dy) * Math.Cos(dr) + Math.Cos(dy) * Math.Sin(dp) * Math.Sin(dr)) + z * (Math.Sin(dy) * Math.Sin(dr) + Math.Cos(dy) * Math.Sin(dp) * Math.Cos(dr));
double y0 = x * (Math.Sin(dy) * Math.Cos(dp)) + y * (Math.Cos(dy) * Math.Cos(dr) + (Math.Sin(dy) * Math.Sin(dp) * Math.Sin(dr))) + z * (-Math.Cos(dy) * Math.Sin(dr) + Math.Sin(dy) * Math.Sin(dp) * Math.Cos(dr));
double z0 = x * (-Math.Sin(dp)) + y * (Math.Cos(dp) * Math.Sin(dr)) + z * (Math.Cos(dp) * Math.Cos(dr));
Vector3 val = new Vector3((float)x0, (float)y0, (float)z0);
return val;
}


static Vector3 RotMat_rev(float x, float y, float z, float dr, float dp, float dy)
{
double xd =
x * (Math.Cos(dy) * Math.Cos(dp)) + y * (Math.Sin(dy) * Math.Cos(dp)) + z * (-Math.Sin(dp));
double yd =
x * (-Math.Sin(dy) * Math.Cos(dr) + Math.Cos(dy) * Math.Sin(dp) * Math.Sin(dr)) + y * (Math.Cos(dy) * Math.Cos(dr) + (Math.Sin(dy) * Math.Sin(dp) * Math.Sin(dr))) + z * (Math.Cos(dp) * Math.Sin(dr));
double zd =
x * (Math.Sin(dy) * Math.Sin(dr) + Math.Cos(dy) * Math.Sin(dp) * Math.Cos(dr)) + y * (-Math.Cos(dy) * Math.Sin(dr) + Math.Sin(dy) * Math.Sin(dp) * Math.Cos(dr)) + z * (Math.Cos(dp) * Math.Cos(dr));

Vector3 val = new Vector3((float)xd, (float)yd, (float)zd);
return val;
}

static Vector3 RotMat_NV(float x, float y, float z, float dr, float dp, float dy)
{
double xd =
x * (Math.Cos(dy) * Math.Cos(dp)) + y * (Math.Sin(dy) * Math.Cos(dp)) + z * (-Math.Sin(dp));
double yd =
x * (-Math.Sin(dy) * Math.Cos(dr) + Math.Cos(dy) * Math.Sin(dp) * Math.Sin(dr)) + y * (Math.Cos(dy) * Math.Cos(dr) + (Math.Sin(dy) * Math.Sin(dp) * Math.Sin(dr))) + z * (Math.Cos(dp) * Math.Sin(dr));
double zd =
x * (Math.Sin(dy) * Math.Sin(dr) + Math.Cos(dy) * Math.Sin(dp) * Math.Cos(dr)) + y * (-Math.Cos(dy) * Math.Sin(dr) + Math.Sin(dy) * Math.Sin(dp) * Math.Cos(dr)) + z * (Math.Cos(dp) * Math.Cos(dr));

Vector3 val = new Vector3((float)xd, (float)yd, (float)zd);
return val;
}


}
プロフィール

二名川(ニナガワ)

Author:二名川(ニナガワ)
ホビーロボットをレトロゲームが発展したものと捉えて楽しく遊び倒します。
子供が夢を見ている時間帯に稼働します。
Qiita記事
電子出版
「コンソロイド ガイドブック」
46107_CONSOLOID_GUID_FACE_200.jpg





■作成中の機体
汎用ヒト型決戦遊具 ~RX計画~
RX-7.5 ゼロタンク
RRf-0.6 ゼニィ
RXM-7.9 ゼムネス
RX-7.5R 量産型ゼロタンク
RX-7.5Fp ファミタンク仮設1号
RX-7.7 ゼロキャノン
RX-7.8 ゼログレイ
SMS-0.1 ゼロライナー
以下続く

RX整備計画(仮)

ブログ内検索
最近の記事
最近のコメント
カテゴリ
月別アーカイブ
リンク