順/逆運動学演算プログラム 公開サンプル ソース(C++言語)
- プログラムの仕様に関してはDocuments/Proposal & Specsを参照して下さい。
- 21/3/31 順・逆運動学演算部 開発計画書に処理内容の記載を追加しましたので、旧IK 3Dソルバのソース コードを削除しました。
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 概要 : RealNoids : IKコア データ
// 作成者 : 市来 博記
// リビジョン : 3.2200
//
//
// COPYRIGHT (C) 2021 NeuralSoft Co.Ltd. (Hiroki Ichiki)
// ALL RIGHT RESERVED
//
////////////////////////////////////////////////////////////////////////////////////////////////////
#ifndef IK_CORE_DATA_H
#define IK_CORE_DATA_H
////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// IKオプション データ
//
struct IkOptionData
{
// 初期姿勢演算方式型
enum class InitType
{
INIT_TYPE_COS_NTHROOT,
};
// CCD固定データ
struct CcdFix
{
// 固定数
rbxSys::Shrink<Int32> mJoints;
Bool mEnaBInInit; // 始端側適用指定（ture : 適用する / false : 適用しない）
Bool mEnaEInInit; // 終端側適用指定（true : 適用する / false : 適用しない）
CcdFix() : mJoints(0, 0), mEnaBInInit(false), mEnaEInInit(false) {}
bool operator==(const CcdFix& r) const
{
return (mJoints == r.mJoints)
&& (mEnaBInInit == r.mEnaBInInit)
&& (mEnaEInInit == mEnaEInInit);
}
bool operator!=(const CcdFix& r) const { return operator==(r); }
};
// デフォルト最大繰り返し回数
static constexpr Int32 scmDefMaxIterations = 20;
// デフォルト閾値
static constexpr Float scmDefThreshold = 0.01;
// デフォルト最大連続角度超過回数
static constexpr Int32 scmDefMaxExcess = 100;
// 初期屈曲係数基礎値初期値
static constexpr Float scmDefInitCoe = 1.0;
// 初期屈曲ウェイト基礎値（最大値）初期値
static constexpr Float scmDefInitWeightMin = 0.0;
// 初期屈曲ウェイト基礎値（最小値）初期値
static constexpr Float scmDefInitWeightMax = 0.0;
// CCD平滑化
static constexpr Float scmDefCcdSmooth = 0.1;
Float mThreshold; // CCD閾値
Int32 mMaxIterations; // CCD最大繰り返し回数
Int32 mMaxExcess; // 最大連続角度超過回数
InitType mInitType; // 初期屈曲姿勢演算法
Float mInitCoe; // 初期屈曲係数基礎値
rbxSys::Span<Float>
mInitWeightSpan; // 初期屈曲ウェイト基礎値（最大・最小値）
Float mCcdSmooth; // CCD平滑化
CcdFix mCcdFix; // CCD固定データ
IkOptionData();
IkOptionData(ENUM_DONT_INITIALIZE v) {}
bool operator==(const IkOptionData& r) const;
bool operator!=(const IkOptionData& r) const { return operator==(r); }
};
////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 屈曲因子データ
//
struct IkBendFactorData
{
// 屈曲基準軸指定型
enum class BendRefAxis
{
FREE = -1,
XP, // X+
XN, // X-
YP, // Y+
YN, // Y-
X = 0,
Y,
};
Int32 mBendRefIndex; // 屈曲基準番号
BendRefAxis mBendRefAxis; // 屈曲基準軸
Float mBendRefTwist; // 屈曲基準ツイスト角度
IkBendFactorData();
bool operator==(const IkBendFactorData& r) const;
bool operator!=(const IkBendFactorData& r) const { return operator==(r); }
};
////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// IKコア データ クラス
//
class IkCoreData
{
protected:
// IKコア データ構築
IkCoreData();
// IKコア データ破棄
virtual ~IkCoreData();
public:
// 最大ジョイント数
static constexpr Int32 scmMaxJoints = 28;
// ジョイント データ
struct IkJointData
{
BaseObject* mModel; // モデル
UInt64 mGuid; // ユニークID
rbxSys::VectorSpan mRotLimit; // 可動範囲（最小/最大角度）
rbxSys::RelRotFrozen mInitRelRotFrozen; // ジョイントの座標変換を固定するローカル マトリクスとオブジェクト相対角度
IkJointData() : mModel(nullptr), mGuid(0) {}
IkJointData(ENUM_DONT_INITIALIZE v) : mRotLimit(v) {}
IkJointData(BaseObject* model, const BaseObject* parent) : mModel(model), mGuid(model->GetGUID())
{
setInitRelRotFrozen(parent);
setRotLimit();
}
// ジョイントの座標変換を固定するローカル マトリクスとオブジェクト相対角度設定
void setInitRelRotFrozen(const BaseObject* parent)
{
DebugAssert(mModel != nullptr);
mInitRelRotFrozen.rbxSys::RelRotFrozen::RelRotFrozen(rbxSys::getFrozenRotMl(mModel, parent), mModel->GetRelRot());
}
// ジョイント角度制限設定
void setRotLimit(void)
{
DebugAssert(mModel != nullptr);
rbxSys::getJointRotLimit(mModel, mRotLimit);
}
};
// 0許容値
static constexpr Float scmTolerance = 1.0E-6;
protected:
// 3のn乗根テーブル（要素数はIKチェーンが脊柱の場合を想定している。）
// IKチェーンのジョイント数がこのテーブルの要素数を超えている場合はpow関数を使用する。
static constexpr Float smNthRootOf3[scmMaxJoints] =
{
3.0, // 3^(1/1)
1.7320508075688772935274463415059, // 3^(1/2)
1.4422495703074083823216383107801, // 3^(1/3)
1.316074012952492460819218901797, // 3^(1/4)
1.2457309396155173259666803366403, // 3^(1/5)
1.2009369551760027266754653873495, // 3^(1/6)
1.1699308127586868864629757255137, // 3^(1/7)
1.1472026904398770894730586135366, // 3^(1/8)
1.1298309639097530326121661042828, // 3^(1/9)
1.1161231740339044344426141383771, // 3^(1/10)
1.1050315033964666965126262991692, // 3^(1/11)
1.0958726911352443801600191280725, // 3^(1/12)
1.0881822434633167374809461526063, // 3^(1/13)
1.081633400352765958567168831875, // 3^(1/14)
1.075989624725345866694666362724, // 3^(1/15)
1.0710754830729144691232476346997, // 3^(1/16)
1.0667581171328452951068613563212, // 3^(1/17)
1.0629350704110543330004157448697, // 3^(1/18)
1.0595260647382752026415391804852, // 3^(1/19)
1.056467308549537861393351452988, // 3^(1/20)
1.0537074720930694092545061496175, // 3^(1/21)
1.0512047866122312784722229950109, // 3^(1/22)
1.0489249176452925003357598908667, // 3^(1/23)
1.0468393817273232390132159319596, // 3^(1/24)
1.0449243511440875747936483441148, // 3^(1/25)
1.0431597401468850482378523040059, // 3^(1/26)
1.0415284982314350450087034118606, // 3^(1/27)
1.0400160577379399090482568974937, // 3^(1/28)
};
// ソルバーの状態
enum class SolveState
{
INIT,
ATTACHED,
SOLVED
} mSolveState;
// バインド タイプ
enum class BindType
{
STRAIGHT,
BEND,
} mBindType;
// 演算回数
Int32 mRealIteration;
// 演算誤差
Float mError;
// バインド タイプ確定
void setBindType(const BaseArray<IkJointData>& ikChain);
public:
// CCD演算回数取得
Int32 getRealIteration(void) { return mRealIteration; }
// CCD演算誤差取得
Float getError(void) { return mError; }
};
#endif // IK_CORE_DATA_H
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 概要 : RealNoids : IKコア データ
// 作成者 : 市来 博記
// リビジョン : 3.2200
//
//
// COPYRIGHT (C) 2021 NeuralSoft Co.Ltd. (Hiroki Ichiki)
// ALL RIGHT RESERVED
//
////////////////////////////////////////////////////////////////////////////////////////////////////
#include "rbxSys4D.h"
#include "ikCoreData.h"
////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// IKオプション データ
//
IkOptionData::IkOptionData() :
mThreshold(scmDefThreshold),
mMaxIterations(scmDefMaxIterations),
mMaxExcess(scmDefMaxExcess),
mInitType(InitType::INIT_TYPE_COS_NTHROOT),
mInitCoe(scmDefInitCoe),
mInitWeightSpan(scmDefInitWeightMin, scmDefInitWeightMax),
mCcdSmooth(scmDefCcdSmooth)
{
}
bool IkOptionData::operator==(const IkOptionData& r) const
{
return (
(mThreshold == r.mThreshold) &&
(mMaxIterations == r.mMaxIterations) &&
(mMaxExcess == r.mMaxExcess) &&
(mInitType == r.mInitType) &&
(mInitCoe == r.mInitCoe) &&
(mInitWeightSpan == r.mInitWeightSpan) &&
(mCcdSmooth == r.mCcdSmooth) &&
(mCcdFix == r.mCcdFix));
}
////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 屈曲因子データ
//
IkBendFactorData::IkBendFactorData() :
mBendRefIndex(1),
mBendRefAxis(BendRefAxis::YP),
mBendRefTwist(0.0)
{
}
bool IkBendFactorData::operator==(const IkBendFactorData& r) const
{
return (
(mBendRefIndex == r.mBendRefIndex) &&
(mBendRefAxis == r.mBendRefAxis) &&
(mBendRefTwist == r.mBendRefTwist));
}
////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// IKコア データ
//
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 機能 : IKコア データ構築
// 備考 :
//
IkCoreData::IkCoreData()
: mSolveState(SolveState::INIT) // ソルバーの状態
, mBindType(BindType::STRAIGHT) // バインド タイプ
, mRealIteration(-1) // 演算回数
, mError(0.0) // 演算誤差
{
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 機能 : IKコア データ破棄
// 備考 :
//
IkCoreData::~IkCoreData()
{
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 機能 : バインド タイプ確定
// 戻り値 : なし
// 備考 : なし
//
void IkCoreData::setBindType(const BaseArray<IkJointData>& ikChain)
{
// 始端(0番)→1番ジョイントへのG座標系ベクトルを基準ベクトルとして求める
const IkJointData& b = ikChain[0];
Matrix mg = b.mModel->GetUpMg() * b.mInitRelRotFrozen.getMl();
Vector o = mg.off;
mg = mg * ikChain[1].mInitRelRotFrozen.getMl();
Vector v0 = (mg.off - o).GetNormalized();
// 2番目以降のジョイントが基準ベクトルの延長線上にあるかどうかを確認
for (Int32 i = 2; i < ikChain.GetCount(); ++i)
{
Vector o = mg.off;
mg = mg * ikChain[i].mInitRelRotFrozen.getMl();
Vector v = (mg.off - o).GetNormalized();
geBreakRet(rbxSys::isEqual(v, v0) == true, mBindType=BindType::BEND);
}
mBindType = BindType::STRAIGHT;
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 概要 : RealNoids : IK 2Dソルバ データ
// 作成者 : 市来 博記
// リビジョン : 3.2200
//
//
// COPYRIGHT (C) 2021 NeuralSoft Co.Ltd. (Hiroki Ichiki)
// ALL RIGHT RESERVED
//
////////////////////////////////////////////////////////////////////////////////////////////////////
#ifndef IK_2D_SOLVER_DATA_H
#define IK_2D_SOLVER_DATA_H
////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// ジョイント データ
//
struct Ik2dJointData
{
// 基本データ
Int32 mNum; // 番号
BaseObject* mModel; // モデル
Matrix mFrozenMl; // 座標変換固定Ml
Matrix mInvFrozenMl; // 座標変換固定逆Ml
Vector mInitRelRot; // 初期姿勢の角度
Bool mEnableRotLimit; // 角度制限状況（true:制限あり/false:制限なし）
rbxSys::Span<Float> mRotLimit; // 角度制限値（最小値/最大値）（IK平面における初期姿勢からの相対角度）
// FKデータ
Vector mFkRot; // FKの角度（初期姿勢からの相対角度）
Vector mFkRot_; // FKの実効角度（強度を考慮したFKの角度）
// 演算データ
Matrix mRelMl; // 相対Ml
Matrix mIkMl; // IK空間座標系Ml
// 2D演算データ
Vector m2dPl; // 2D位置（計算前）
Vector m2dPl_; // 2D位置（計算後）
Float m2dLl; // 2Dジョイント間長（IK平面に投影した長さ）
Float m2dLlFromB; // 2D始端からの長さ（IK平面に投影した長さ）
Float m2dAngle; // 2D絶対角度(演算前)(基準=IK空間座標系のX軸)
Float m2dAngle_; // 2D絶対角度(演算後)(基準=IK空間座標系のX軸)
Float m2dRelRot; // 2D相対角度（2D仮想ジョイント相対）
Float m2dInitWeight; // 2D初期屈曲ウェイト値
Vector m2dPl0; // FK成分なしの2D初期位置
Float m2dFKRelRot; // FK成分の2D相対角度
Int32 mExcess; // 連続角度制限超過回数
// 演算結果データ
Matrix mMg_; // Mg
Vector mRelRot_; // 相対角度
Ik2dJointData() {}
};
#endif // IK_2D_SOLVER_DATA_H
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 概要 : RealNoids : IK 2Dソルバ クラス
// 作成者 : 市来 博記
// リビジョン : 3.2200
//
//
// COPYRIGHT (C) 2021 NeuralSoft Co.Ltd. (Hiroki Ichiki)
// ALL RIGHT RESERVED
//
////////////////////////////////////////////////////////////////////////////////////////////////////
#ifndef IK_2D_SOLVER_H
#define IK_2D_SOLVER_H
#include "ikCoreData.h"
#include "ik2dSolverData.h"
////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// IK 2Dソルバー クラス
//
class Ik2dSolver : public IkCoreData, protected IkOptionData, protected IkBendFactorData
{
private:
// IK空間座標系Mg
Matrix mIkMg;
// IK空間座標系逆Mg
Matrix mInvIkMg;
BaseArray<Ik2dJointData> mIkJoints;
// IKチェーンの2D長
Float m2dTotalLen;
// 最初の回転可能ジョイントの番号
Int32 mBeginRotJ;
// 最後の回転可能ジョイントの番号
Int32 mEndRotJ;
// 回転可能ジョイント数
Int32 mRotJCnt;
// コントローラの位置（IK空間座標系）
Vector mCtrlPl;
// ポールの位置（IK空間座標系）
Vector mPolePl;
public:
// IK 2Dソルバー構築
Ik2dSolver();
// IKオプション設定
void setOption(const IkOptionData& od);
// IKチェーン接続
Bool attach(const BaseArray<IkJointData>& ikChain);
// 屈曲因子設定
Bool setBendFactor(const IkBendFactorData& bfd);
// IK解決
Bool solve(
const BaseArray<Vector>& fk, Float fkStrength,
const BaseObject* cp, const BaseObject* pp
);
// IKチェーン姿勢更新
void update(Float fk = 0.0);
// IKチェーンG位置取得
void getIkPgs(BaseArray<Vector>& pgs);
private:
////////////////////////////////////////////////////////////////////////////////
// IKチェーン接続系
// 基本データ設定
void setBaseData(const BaseArray<IkJointData>& ikChain);
////////////////////////////////////////////////////////////////////////////////
// IK解決系
// IK空間座標系確定
void setIkMg(const BaseObject* op, const BaseObject* cp, const BaseObject* pp);
// FK成分確定
void setFkData(const BaseArray<Vector>& fk, Float fkStrength);
// IK平面確定
void setIkPlane(void);
// 初期姿勢設定
void setInitPose(void);
// 簡易照準姿勢設定（始端、屈曲基準、終端のみをコントローラに向ける）
void aimIkChainBPE(void);
// FK成分を適用
void setFKRot(void);
// 照準姿勢設定（全てのジョイントをコントローラに向ける）
void aimIkChain(void);
// 2D演算初期データ確定
void set2DCalcInitData(void);
// 初期屈曲姿勢設定
void setStartPose(void);
// 2D初期屈曲ウェイト確定
void set2DInitWeight(Float btoc);
// 2DFK成分適用
void set2DFKRelRot(void);
// 2D角度制限適用
void setRotLimit(void);
// CCD（Cyclic Coordinate Descent法）演算
Bool ccd(Float strength);
// IK解決をスキップ
void skip(void);
// IK解決演算結果設定
void setIkChainMg(void);
// IK空間座標系Ml確定
void setIkMl(void);
// 2D角度変換（絶対角度 -> 相対角度）
// 絶対角度：IK平面X軸からの角度
// 相対角度：親2D仮想ジョイントに対する相対角度
void cnvtAngleToRelRot(void);
// 2D角度変換（相対角度 -> 絶対角度）
// 相対角度：親2D仮想ジョイントに対する相対角度
// 絶対角度：IK平面X軸からの角度
void cnvtRelRotToAngle(void);
// 2D位置更新
void update2DPos(void);
// 連続角度制限超過回数
void clearExcess(void);
// IKチェーン姿勢更新（IK+FK/FK混合）
void updateIkFk_Fk(Float fk = 0.0);
};
#endif // __IK_2D_SOLVER_H__
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 概要 : RealNoids : IK 2D ソルバ クラス 外部関数定義
// 作成者 : 市来 博記
// リビジョン : 3.2200
//
//
// COPYRIGHT (C) 2021 NeuralSoft Co.Ltd. (Hiroki Ichiki)
// ALL RIGHT RESERVED
//
////////////////////////////////////////////////////////////////////////////////////////////////////
#include "rbxSys4D.h"
#include "ik2DSolver.h"
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 機能 : IK 2Dソルバ構築
// 備考 : なし
//
Ik2dSolver::Ik2dSolver() :
m2dTotalLen(0.0) // IKチェーンの2D長
, mBeginRotJ(-1) // 最初の回転可能ジョイントの番号
, mEndRotJ(-2) // 最後の回転可能ジョイントの番号
, mRotJCnt(0) // 回転可能ジョイント数
{
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 機能 : IKオプション設定
// 戻り値 : なし
// 備考 :
//
void Ik2dSolver::setOption(const IkOptionData& od)
{
*static_cast<IkOptionData*>(this) = od;
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 機能 : IKチェーン接続
// 戻り値 : 処理結果
// : true : 正常終了
// : false : 異常終了
// 備考 : なし
//
Bool Ik2dSolver::attach(const BaseArray<IkJointData>& ikChain)
{
mSolveState = SolveState::INIT;
Int32 count = Int32(ikChain.GetCount());
geBreakRet(count >= 2 && count <= scmMaxJoints, mIkJoints.Reset(), false);
// ジョイント データ領域確保
if (count != Int32(mIkJoints.GetCount()))
{
mIkJoints.Resize(count);
geBreakRet(Int32(mIkJoints.GetCount()) == count, mIkJoints.Reset(), false);
}
// バインド タイプ確定
setBindType(ikChain);
// 基本データ設定
setBaseData(ikChain);
// CCD終了ステータスを初期化
mRealIteration = -1;
mError = 0.0;
// ソルバの状態を「IKチェーン接続」に設定
mSolveState = SolveState::ATTACHED;
return true;
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 機能 : 屈曲因子設定
// 戻り値 : 処理結果
// : true : 正常終了
// : false : 異常終了
// 備考 : 0≦屈曲基準の番号＜IKチェーンのジョイント数
//
Bool Ik2dSolver::setBendFactor(const IkBendFactorData& bfd)
{
geBreakRet(mSolveState >= SolveState::ATTACHED, , false);
*static_cast<IkBendFactorData*>(this) = bfd;
mBendRefIndex = rbxSys::trim(mBendRefIndex, 0, Int32(mIkJoints.GetCount()) - 1);
return true;
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 機能 : IK解決
// 戻り値 : 処理結果
// : true : 正常終了
// : false : 異常終了
// 備考 : コントローラとポールの位置はG座標系
//
Bool Ik2dSolver::solve(
const BaseArray<Vector>& fk, Float fkStrength,
const BaseObject* cp, const BaseObject* pp)
{
geBreakRet(mSolveState >= SolveState::ATTACHED, , false);
geBreakRet(Int32(fk.GetCount()) == Int32(mIkJoints.GetCount()), , false);
// CCD終了ステータスを初期化
mError = 0.0;
// IK解決済みの場合は連続角度制限超過回数クリアー
if (mSolveState == SolveState::SOLVED)
{
clearExcess();
}
mRealIteration = -1;
// IK空間座標系確定
setIkMg(mIkJoints.GetFirst()->mModel, cp, pp);
// FK成分確定
setFkData(fk, fkStrength);
// IK平面確定
setIkPlane();
// FK成分を適用
setFKRot();
// 2D演算初期データ確定
set2DCalcInitData();
// CCD処理回数が-1以下の場合は処理を終了（照準姿勢となる。）
geBreakRet(mMaxIterations >= 0, skip(), true);
// 初期屈曲姿勢設定
setStartPose();
// 最大繰り返し回数を上限として、終端とコントローラ間の距離が閾値より小さくなるまでCCD演算を繰り返す
Int32 i;
for (i = 0; i < mMaxIterations; ++i)
{
// CCD法で姿勢を変更
bool changed = ccd(Min((i + 1)*mCcdSmooth, 1.0));
if (!changed) { ++i; break; }
// 終端とコントローラの差を取得して終了判定
mError = (mCtrlPl - mIkJoints.GetLast()->m2dPl_).GetLength();
if (mError < mThreshold) { ++i; break; }
}
// IK解決までのCCD演算回数を設定
mRealIteration = i;
// IK解決演算結果設定
setIkChainMg();
// ソルバの状態を「IK解決済み」に設定
mSolveState = SolveState::SOLVED;
return true;
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 機能 : IKチェーン姿勢更新
// 戻り値 : なし
// 備考 : なし
//
// ジョイント オブジェクトに対する最終の軸角度設定方式
#define UPDATE_BY_MG 0 // 軸角度設定をMgで行う
#define UPDATE_BY_ROT_PI 1 // 軸角度設定をオブジェクト相対角度で行う
#define UPDATE_METHOD UPDATE_BY_ROT_PI
void Ik2dSolver::update(Float fk)
{
if (mSolveState == SolveState::SOLVED)
{
if (fk < scmTolerance)
{ // IK+RelFK
for (Int32 i = 0; i < Int32(mIkJoints.GetCount()) - 1; ++i)
{ Ik2dJointData& j = mIkJoints[i];
#if UPDATE_METHOD == UPDATE_BY_MG
j.mModel->SetMg(j.mMg_);
#else
j.mModel->SetRelRot(j.mRelRot_);
#endif
}
}
else
if (fk >= 1.0 - scmTolerance)
{ // AbsFK
for (Int32 i = 0; i < Int32(mIkJoints.GetCount()) - 1; ++i)
{ Ik2dJointData& j = mIkJoints[i];
j.mModel->SetRelRot(j.mInitRelRot + j.mFkRot);
}
}
else
{ // IK+RelFK + AbsFK
// (IK+RelFK)*(1.0 -fk) + AbsFK*fk
updateIkFk_Fk(fk);
}
}
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 機能 : IKチェーン姿勢更新（IK+FK/FK混合）
// 戻り値 : なし
// 備考 : なし
//
void Ik2dSolver::updateIkFk_Fk(Float fk)
{
#if UPDATE_METHOD == UPDATE_BY_MG
Matrix ikfk_fkUpMg(mIkMg.off, mIkJoints.GetFirst()->mModel->GetUpMg().sqmat);
#endif
for (Int32 i = 0; i < Int32(mIkJoints.GetCount()) - 1; ++i)
{
Ik2dJointData& j = mIkJoints[i];
Vector ikfk_fkRot = j.mRelRot_*(1 - fk) + (j.mInitRelRot + j.mFkRot)*fk;
#if UPDATE_METHOD == UPDATE_BY_MG
Matrix ikfk_fkMl = j.mFrozenMl*HPBToMatrix(ikfk_fkRot, ROTATIONORDER::HPB);
ikfk_fkUpMg = ikfk_fkUpMg*ikfk_fkMl;
j.mModel->SetMg(ikfk_fkUpMg);
#else
j.mModel->SetRelRot(ikfk_fkRot);
#endif
}
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 機能 : IKチェーン G位置取得
// 戻り値 : なし
// 備考 : なし
//
void Ik2dSolver::getIkPgs(BaseArray<Vector>& pgs)
{
if (mSolveState == SolveState::SOLVED)
{
pgs.Resize(mIkJoints.GetCount());
for (Int32 i = 0; i < Int32(mIkJoints.GetCount()); ++i)
{
pgs[i] = mIkJoints[i].mMg_.off;
}
}
else
{
pgs.Reset();
}
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 概要 : RealNoids : IK 2D ソルバ クラス（内部関数）
// 作成者 : 市来 博記
// リビジョン : 3.2200
//
//
// COPYRIGHT (C) 2021 NeuralSoft Co.Ltd. (Hiroki Ichiki)
// ALL RIGHT RESERVED
//
////////////////////////////////////////////////////////////////////////////////////////////////////
#include "rbxSys4D.h"
#include "ik2DSolver.h"
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 機能 : 基本データ設定
// 戻り値 : なし
// 備考 : 基本データ設定時に最初と最後の回転可能ジョイントの番号と
// : 回転可能ジョイント数を確定
//
void Ik2dSolver::setBaseData(const BaseArray<IkJointData>& ikChain)
{
// 最初と最後の回転可能ジョイントの番号をｰ1に初期化
rbxSys::SetBeginEnd<Int32> beginEndRotJ(-1);
// 回転可能ジョイント数を0に初期化
mRotJCnt = 0;
for (Int32 i = 0; i < Int32(mIkJoints.GetCount()); ++i)
{
const IkJointData& d = ikChain[i];
Ik2dJointData& j = mIkJoints[i];
j.mNum = i;
j.mModel = d.mModel;
j.mFrozenMl = d.mInitRelRotFrozen.mFrozenMl;
j.mInvFrozenMl = ~j.mFrozenMl;
j.mInitRelRot = d.mInitRelRotFrozen.mRelRot;
j.mEnableRotLimit = d.mRotLimit.mEnableH;
j.mRotLimit.mMin = d.mRotLimit.mMin.x;
j.mRotLimit.mMax = d.mRotLimit.mMax.x;
j.mExcess = 0;
// 回転可能ジョイント数と、最初と最後の回転可能ジョイントの番号を更新
if ((j.mEnableRotLimit == false) || ((j.mRotLimit.mMax - j.mRotLimit.mMin) >= scmTolerance))
{
++mRotJCnt;
beginEndRotJ = i;
}
}
// 最初と最後の回転可能ジョイントの番号を設定
mBeginRotJ = beginEndRotJ.getBegin();
mEndRotJ = beginEndRotJ.getEnd();
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 機能 : IK空間座標系確定
// : 確定する内容
// : IK空間座標系Mg
// : IK空間座標系逆Mg
// : IK空間座標系コントローラの位置
// : IK空間座標系ポールの位置
// 戻り値 : なし
// 備考 : IK空間座標系Mgの内容
// 原点:IK始端
// X軸:IK始端 -> コントローラへの単位ベクトル
// Z軸:X軸とIK始端 -> ポールの両方に直角な単位ベクトル
// Y軸:X軸とZ軸のベクトルの両方に直角な単位ベクトル
//
void Ik2dSolver::setIkMg(const BaseObject* op, const BaseObject* cp, const BaseObject* pp)
{
// IK空間座標系Mgを設定
Matrix bmg = op->GetMg();
Matrix cmg = cp->GetMg();
Matrix pmg = pp->GetMg();
Vector pv = pmg.off - bmg.off;
Vector xAxis = cmg.off - bmg.off;
Vector zAxis = Cross(xAxis, pv);
Vector yAxis = Cross(zAxis, xAxis);
mIkMg = Matrix(bmg.off, xAxis, yAxis, zAxis).GetNormalized();
mInvIkMg = ~mIkMg;
// コントローラとポールのIK空間座標系の位置を設定
mCtrlPl = mInvIkMg * cmg.off;
mPolePl = mInvIkMg * pmg.off;
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 機能 : FK成分確定
// 戻り値 : なし
// 備考 : なし
//
void Ik2dSolver::setFkData(const BaseArray<Vector>& fk, Float fkStrength)
{
for (Int32 i = 0; i < Int32(mIkJoints.GetCount()); ++i)
{
Ik2dJointData& j = mIkJoints[i];
j.mFkRot = fk[i];
j.mFkRot_ = j.mFkRot*fkStrength;
}
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 機能 : IK平面確定
// : 手順
// : 初期姿勢 -> 簡易照準姿勢（始端、屈曲基準、終端のみをコントローラに向ける）
// : 屈曲基準の基準軸をポールに向けるために必要な角度 + 屈曲ツイスト角度分、
// : 始端のZ軸を回転
// 戻り値 : なし
// 備考 : なし
//
void Ik2dSolver::setIkPlane(void)
{
// 初期姿勢設定
setInitPose();
// 簡易照準姿勢設定（始端、屈曲基準、終端のみをコントローラに向ける）
aimIkChainBPE();
// ツイスト設定
if (mBendRefAxis >= BendRefAxis::XP)
{ // 屈曲基準の基準軸をポールに向ける
// 基準軸ベクトルを取得
Vector bendRefAxis = rbxSys::getMatrixAxis(mIkJoints[mBendRefIndex].mIkMl, (N(mBendRefAxis) / 2) + 1);
if ((N(mBendRefAxis) % 2) != 0) { bendRefAxis *= -1; }
// 基準軸ベクトルと「始端→ポール」ベクトルの角度 + 屈曲ツイスト角度分、
// 始端をIK空間座標系X軸回転
// （ZY平面上での基準軸ベクトルからatan2で角度を求めるとZ軸からの角度となる為、
// 基準軸ベクトルのZ値とY値を入れ替えて計算する）
Ik2dJointData& bj = *mIkJoints.GetFirst();
Matrix rotX = MatrixRotX(ATan2(bendRefAxis.z, bendRefAxis.y) + mBendRefTwist);
bj.mIkMl = rotX*bj.mIkMl;
}
else
{ // フリー ツイスト
Ik2dJointData& bj = *mIkJoints.GetFirst();
Matrix rotX = MatrixRotX(mBendRefTwist);
bj.mIkMl = rotX*bj.mIkMl;
}
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 機能 : 初期姿勢設定
// 戻り値 : なし
// 備考 : なし
//
void Ik2dSolver::setInitPose(void)
{
for (Int32 i = 0; i < Int32(mIkJoints.GetCount()); ++i)
{
Ik2dJointData& j = mIkJoints[i];
j.mRelMl = HPBToMatrix(j.mInitRelRot, ROTATIONORDER::HPB);
}
Ik2dJointData& bj = *mIkJoints.GetFirst();
if (mBendRefAxis == BendRefAxis::FREE)
{
Matrix ml = mInvIkMg*(bj.mModel->GetUpMg()*bj.mFrozenMl*bj.mRelMl);
bj.mFrozenMl.off = Vector();
bj.mInvFrozenMl = ~bj.mFrozenMl;
bj.mRelMl = bj.mInvFrozenMl*ml;
}
else
{
bj.mFrozenMl.off = Vector();
bj.mInvFrozenMl = ~bj.mFrozenMl;
}
// IK空間座標系Ml確定
setIkMl();
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 機能 : 簡易照準姿勢設定（始端、屈曲基準、終端のみをコントローラに向ける）
// 戻り値 : なし
// 備考 : なし
//
void Ik2dSolver::aimIkChainBPE(void)
{
const Vector& ePl = mIkJoints.GetLast()->mIkMl.off;
Ik2dJointData& bj = *mIkJoints.GetFirst();
Ik2dJointData& rj = mIkJoints[mBendRefIndex];
Ik2dJointData& ej = *mIkJoints.GetLast();
// IKチェーンをIK空間座標系Y軸回転して終端をコントローラに向ける
Matrix rotY = MatrixRotY(-ATan2(ePl.z, ePl.x));
bj.mIkMl = rotY*bj.mIkMl;
rj.mIkMl = rotY*rj.mIkMl;
ej.mIkMl = rotY*ej.mIkMl;
// IKチェーンをIK空間座標系Z軸回転して終端をコントローラに向ける
Matrix rotZ = MatrixRotZ(ATan2(ePl.y, ePl.x));
bj.mIkMl = rotZ*bj.mIkMl;
rj.mIkMl = rotZ*rj.mIkMl;
ej.mIkMl = rotZ*ej.mIkMl;
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 機能 : FK成分を適用
// 戻り値 : なし
// 備考 : なし
//
void Ik2dSolver::setFKRot(void)
{
// 始端の相対Mlを設定（FK成分を適用する）
Ik2dJointData& bj = *mIkJoints.GetFirst();
bj.mRelMl = bj.mIkMl*MatrixRotY(bj.mFkRot_.x)*MatrixRotX(bj.mFkRot_.y)*MatrixRotZ(bj.mFkRot_.z);
// 始端の次のジョイントから終端までの相対Mlを設定（FK成分を適用する）
const Ik2dJointData* preJ = &bj;
for (Int32 i = 1; i < Int32(mIkJoints.GetCount()); ++i)
{
Ik2dJointData& j = mIkJoints[i];
Vector preJtFkRot = j.mFrozenMl.sqmat * preJ->mFkRot_;
j.mRelMl = HPBToMatrix(j.mInitRelRot - preJtFkRot + j.mFkRot_, ROTATIONORDER::HPB);
preJ = &j;
}
// IK空間座標系Ml確定
setIkMl();
// 照準姿勢設定（全てのジョイントをコントローラに向ける）
aimIkChain();
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 機能 : 照準姿勢設定（全てのジョイントをコントローラに向ける）
// 戻り値 : なし
// 備考 : なし
//
void Ik2dSolver::aimIkChain(void)
{
const Vector& ePl = mIkJoints.GetLast()->mIkMl.off;
// IKチェーンをIK空間座標系Y軸回転
Matrix rotY = MatrixRotY(-ATan2(ePl.z, ePl.x));
for (Ik2dJointData& j : mIkJoints) { j.mIkMl = rotY*j.mIkMl; }
// IKチェーンをIK空間座標系Z軸回転
Matrix rotZ = MatrixRotZ(ATan2(ePl.y, ePl.x));
for (Ik2dJointData& j : mIkJoints) { j.mIkMl = rotZ*j.mIkMl; }
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 機能 : 2D演算初期データ確定
// 戻り値 : なし
// 備考 : なし
//
void Ik2dSolver::set2DCalcInitData(void)
{
// 2D位置を設定
for (Ik2dJointData& j : mIkJoints)
{
j.m2dPl = j.mIkMl.off;
j.m2dPl.z = 0.0;
}
// 始端から終端の前のジョイントまで2D角度と2D長を設定
m2dTotalLen = 0;
Ik2dJointData* preJ = mIkJoints.GetFirst();
for (Int32 i = 1; i < Int32(mIkJoints.GetCount()); ++i)
{
Ik2dJointData& j = mIkJoints[i];
// 次のジョイントまでの2D長を設定
Vector v = j.m2dPl - preJ->m2dPl;
Float l = v.GetLength();
preJ->m2dLl = l;
// 始端から当該ジョイントまでの2D長を設定
preJ->m2dLlFromB = m2dTotalLen;
// IKチェーンの2D長を更新
m2dTotalLen += l;
// 2D絶対角度（演算前/基準=X軸）を設定
preJ->m2dAngle = ATan2(v.y, v.x);
// 次のジョイントの処理へ
preJ = &j;
}
// 終端の2D角度、2D長を設定
Ik2dJointData& j = *(mIkJoints.GetLast());
j.m2dLl = 0.0;
j.m2dLlFromB = m2dTotalLen;
j.m2dAngle = preJ->m2dAngle;
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 機能 : 初期屈曲姿勢設定
// 戻り値 : なし
// 備考 : なし
//
void Ik2dSolver::setStartPose(void)
{
// 2D角度変換（絶対角度 -> 相対角度）
cnvtAngleToRelRot();
// 支端の次のジョイントから終端の前のジョイント数を求める
Int32 jc = Int32(mIkJoints.GetCount()) - 2 - mBeginRotJ;
if (jc > 0)
{
// 「始端→コントローラ」ベクトル、基本屈曲角度、初期屈曲係数を求める
Float btoc = (mCtrlPl - mIkJoints.GetFirst()->m2dPl).GetLength();
Float baseRot = ACos(btoc * Inverse(m2dTotalLen));
Float coe = (1.0 - baseRot*Inverse(PI05))*(1.0 - mInitCoe) + mInitCoe;
// 「終端の前→終端」の2D角度が「始端→始端の次」の角度×-1 にするための3のn乗根と
// n乗値の初期値を設定
Float a = (jc <= _countof(smNthRootOf3)) ? smNthRootOf3[jc - 1] : Pow(3.0, Inverse(Float(jc)));
Float a_ = 1.0;
// 2D初期屈曲ウェイト確定
set2DInitWeight(btoc);
// 初期屈曲姿勢を設定するジョイントの範囲を確定
Int32 i = ((mCcdFix.mEnaBInInit == false) || (mBeginRotJ > mCcdFix.mJoints.mBegin)) ? mBeginRotJ : mCcdFix.mJoints.mBegin;
Int32 lastNext = Int32(mIkJoints.GetCount()) - 1;
if (mCcdFix.mEnaEInInit == true) { lastNext -= mCcdFix.mJoints.mEnd; }
// 設定をスキップするジョイント分、n乗値を更新
if (i > mBeginRotJ)
{
for (Int32 l = mBeginRotJ; l < i; ++l) { a_ *= a; }
}
// IKチェーンに初期屈曲姿勢の2D角度を設定
Float preRelRot = 0.0;
auto set = [](Float& r, Float v) { r = v; }; void(*setFunc)(Float& r, Float v) = set;
auto add = [](Float& r, Float v) { r += v; }; void(*addFunc)(Float& r, Float v) = add;
void (*update)(Float& r, Float v) = geTernary(mBindType == BindType::STRAIGHT, addFunc, setFunc);
for (; i < lastNext; ++i)
{
Ik2dJointData& j = mIkJoints[i];
Float relRot = baseRot - (baseRot*(a_ - 1.0))*j.m2dInitWeight;
update(j.m2dRelRot, (relRot - preRelRot) * coe);
preRelRot = relRot;
a_ *= a;
}
}
if (mBindType == BindType::BEND)
{
// 2DFK成分適用
// （前段の処理は初期姿勢→初期姿勢+FK成分→2D投影→2D直線配置→2D屈曲である為、FKの2D成分のみを適用する。）
set2DFKRelRot();
}
// 2D角度制限適用
setRotLimit();
// 2D角度変換（相対角度 -> 絶対角度）
cnvtRelRotToAngle();
// 2D位置を更新
update2DPos();
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 機能 : 2D初期屈曲ウェイト確定
// 戻り値 : なし
// 備考 : なし
//
void Ik2dSolver::set2DInitWeight(Float btoc)
{
const Ik2dJointData& sj = mIkJoints[mBeginRotJ];
// 支端から終端までの2D長を求める
Float stoe = m2dTotalLen - sj.m2dLlFromB;
// 支端からウェイト最大位置までの2D長を求める
Float stoa = Max(btoc - sj.m2dLlFromB, stoe*0.5);
// 始端からウェイト最大位置までの2D長を求める
Float btoa = sj.m2dLlFromB + stoa;
// ジョイントからウェイト最大位置までの2D長をキーとするウェイト値を設定
Float invStoe = Inverse(stoe);
Float weightSpan = mInitWeightSpan.getSpan();
for (Int32 i = mBeginRotJ; i < Int32(mIkJoints.GetCount()) - 1; ++i)
{
Ik2dJointData& j = mIkJoints[i];
// 当該ジョイントとウェイト最大位置までの2D長を求める
Float d = Abs(btoa - j.m2dLlFromB);
// ウェイト値を求める
Float c = 1.0 - (d * invStoe);
j.m2dInitWeight = 1.0 + (mInitWeightSpan.mMin + c*c*c*weightSpan);
}
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 機能 : 2DFK成分適用
// 戻り値 : なし
// 備考 : なし
//
void Ik2dSolver::set2DFKRelRot(void)
{
// 相対Mlを設定
Matrix upMg = mIkMg;
for (Int32 i = 0; i < Int32(mIkJoints.GetCount()); ++i)
{
Ik2dJointData& j = mIkJoints[i];
Matrix mg = mIkMg * j.mIkMl;
Matrix ml = ~upMg * mg;
j.mRelMl = j.mInvFrozenMl*ml;
upMg = mg;
}
// 相対MlのFK成分を除去
Ik2dJointData& bj = *mIkJoints.GetFirst();
bj.mRelMl = bj.mRelMl*MatrixRotZ(-bj.mFkRot_.z);
bj.mRelMl = bj.mRelMl*MatrixRotX(-bj.mFkRot_.y);
bj.mRelMl = bj.mRelMl*MatrixRotY(-bj.mFkRot_.x);
const Ik2dJointData* _preJ = &bj;
for (Int32 i = 1; i < Int32(mIkJoints.GetCount()); ++i)
{
Ik2dJointData& j = mIkJoints[i];
Vector preJtFkRot = j.mFrozenMl.sqmat * _preJ->mFkRot_;
j.mRelMl = j.mRelMl * MatrixRotZ(preJtFkRot.z - j.mFkRot_.z);
j.mRelMl = j.mRelMl * MatrixRotX(preJtFkRot.y - j.mFkRot_.y);
j.mRelMl = j.mRelMl * MatrixRotY(preJtFkRot.x - j.mFkRot_.x);
_preJ = &j;
}
// FK成分なしの2D初期位置を設定
upMg = mIkMg;
for (Int32 i = 0; i < Int32(mIkJoints.GetCount()); ++i)
{
Ik2dJointData& j = mIkJoints[i];
upMg = upMg*j.mFrozenMl*j.mRelMl;
j.m2dPl0 = (mInvIkMg * upMg).off;
j.m2dPl0.z = 0.0;
}
// FK成分の2D相対角度を確定
Float relRot = 0.0;
Ik2dJointData* preJ = mIkJoints.GetFirst();
for (Int32 i = 1; i < Int32(mIkJoints.GetCount()); ++i)
{
Ik2dJointData& j = mIkJoints[i];
Vector v0 = (j.m2dPl0 - preJ->m2dPl0).GetNormalized();
Vector v = (j.m2dPl - preJ->m2dPl).GetNormalized();
Float sign = rbxSys::isRevRotXY(v0, v) ? -1.0 : 1.0;
preJ->m2dFKRelRot = ACos(Dot(v0, v))*sign - relRot;
relRot += preJ->m2dFKRelRot;
preJ = &j;
}
preJ->m2dFKRelRot = 0.0;
// 2D相対角度を適用
for (Int32 i = 0; i < Int32(mIkJoints.GetCount()) - 1; ++i)
{
Ik2dJointData& j = mIkJoints[i];
j.m2dRelRot += j.m2dFKRelRot;
}
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 機能 : 2D角度制限適用
// 戻り値 : なし
// 備考 : なし
//
void Ik2dSolver::setRotLimit(void)
{
Ik2dJointData* j = mIkJoints.GetFirst();
for (Int32 i = 1; i < Int32(mIkJoints.GetCount()) - 1; ++i)
{
Ik2dJointData& nj = mIkJoints[i];
if (j->mEnableRotLimit == true)
{
Float overRot =
j->m2dRelRot -
rbxSys::getSafeRotDeg(0.0, j->m2dRelRot, j->mRotLimit.mMin, j->mRotLimit.mMax, scmTolerance);
if (Abs(overRot) > scmTolerance)
{
j->m2dRelRot -= overRot;
nj.m2dRelRot += overRot;
++j->mExcess;
}
}
j = &nj;
}
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 機能 : CCD（Cyclic Coordinate Descent法）演算
// 戻り値 : 終了ステータス
// : true : ジョイント角度の変更あり
// : false : ジョイント角度の変更なし
// 備考 : なし
//
Bool Ik2dSolver::ccd(Float strength)
{
Bool changed = false;
Vector e = mIkJoints.GetLast()->m2dPl_;
for (Int32 i = Int32(mIkJoints.GetCount()) - 2 - mCcdFix.mJoints.mEnd; i >= mCcdFix.mJoints.mBegin; --i)
{
Ik2dJointData& j = mIkJoints[i];
if (j.mExcess < mMaxExcess)
{
Vector toE = e - j.m2dPl_;
Vector toC = mCtrlPl - j.m2dPl_;
Vector toE_Unit = toE.GetNormalized();
Vector toC_Unit = toC.GetNormalized();
// 当該ジョイントを基準とする終端へのベクトルとコントローラへのベクトルの2D角度分
// 当該ジョイントを2D回転
Float sign = rbxSys::isRevRotXY(toE_Unit, toC_Unit) ? -1.0 : 1.0;
Float realRot = ACos(Dot(toE_Unit, toC_Unit))*sign;
Float safeRot = 0.0;
if (j.mEnableRotLimit == false)
{
safeRot = realRot*strength;
}
else
{
Bool exceeded = false;
safeRot =
rbxSys::getSafeRotDegEx(
0.0, j.m2dRelRot + realRot*strength, j.mRotLimit.mMin, j.mRotLimit.mMax,
exceeded, scmTolerance) - j.m2dRelRot;
j.mExcess = geTernary(exceeded == true, j.mExcess + 1, 0);
}
if (Abs(safeRot) > scmTolerance)
{
j.m2dRelRot += safeRot;
// 終端の2D位置を更新
if (Abs(safeRot - realRot) < scmTolerance)
{
e = j.m2dPl_ + toC_Unit*toE.GetLength();
}
else if (Abs(safeRot) >= scmTolerance)
{
e = j.m2dPl_ + MatrixRotZ(-safeRot)*toE;
}
changed = true;
}
}
}
if (changed == true)
{
// 2D角度変換（相対角度 -> 絶対角度）
cnvtRelRotToAngle();
// 2D位置を更新
update2DPos();
return true;
}
return false;
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 機能 : IK解決をスキップ
// 戻り値 : なし
// 備考 : なし
//
void Ik2dSolver::skip(void)
{
for (Ik2dJointData& j : mIkJoints)
{
j.m2dAngle_ = j.m2dAngle;
}
// 2D位置を更新
update2DPos();
// IK解決演算結果設定
setIkChainMg();
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 機能 : IK解決演算結果設定
// 戻り値 : なし
// 備考 : なし
//
void Ik2dSolver::setIkChainMg(void)
{
Matrix upMg(mIkMg.off, mIkJoints.GetFirst()->mModel->GetUpMg().sqmat);
for (Int32 i = 0; i < Int32(mIkJoints.GetCount()); ++i)
{
Ik2dJointData& j = mIkJoints[i];
// 回転Mを生成
Matrix rotZm = MatrixRotZ(-(j.m2dAngle_ - j.m2dAngle));
// ジョイントの原点を中心にするための移動Mを生成
Matrix ml = j.mIkMl;
const Vector& p = j.m2dPl_;
Matrix mvM = MatrixMove(Vector(p.x, p.y, ml.off.z));
ml.off = Vector(0);
// ジョイントのMgを生成
j.mMg_ = (mIkMg*mvM*rotZm)*ml;
// オブジェクト相対角度を算出
Matrix absMl = ~upMg*j.mMg_;
Matrix relMl = j.mInvFrozenMl*absMl;
j.mRelRot_ = rbxSys::getNearRot(rbxSys::getRotPI(MatrixToHPB(relMl, ROTATIONORDER::HPB)), j.mModel->GetRelRot());
upMg = j.mMg_;
}
mSolveState = SolveState::SOLVED;
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 機能 : IK空間座標系Ml確定
// 戻り値 : なし
// 備考 : なし
//
void Ik2dSolver::setIkMl(void)
{
Matrix upMg = mIkMg;
for (Ik2dJointData& j : mIkJoints)
{
Matrix mg = upMg*j.mFrozenMl*j.mRelMl;
j.mIkMl = mInvIkMg*mg;
upMg = mg;
}
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 機能 : 2D角度変換（絶対角度 -> 相対角度）
// : 絶対角度：IK平面X軸からの角度
// : 相対角度：親ジョイントに対する2D相対角度
// 戻り値 : なし
// 備考 : なし
//
void Ik2dSolver::cnvtAngleToRelRot(void)
{
Float angle = 0.0;
for (Ik2dJointData& j : mIkJoints)
{
j.m2dRelRot = j.m2dAngle - angle;
angle = j.m2dAngle;
}
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 機能 : 2D角度変換（相対角度 -> 絶対角度）
// : 相対角度：親ジョイントに対する2D相対角度
// : 絶対角度：IK平面X軸からの角度
// 戻り値 : なし
// 備考 : なし
//
void Ik2dSolver::cnvtRelRotToAngle(void)
{
Float angle = 0.0;
for (Ik2dJointData& j : mIkJoints)
{
angle += j.m2dRelRot;
j.m2dAngle_ = angle;
}
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 機能 : 2D位置更新
// 戻り値 : なし
// 備考 : なし
//
void Ik2dSolver::update2DPos(void)
{
Vector p = mIkJoints.GetFirst()->m2dPl;
for (Ik2dJointData& j : mIkJoints)
{
j.m2dPl_ = p;
p.x += Cos(j.m2dAngle_) * j.m2dLl;
p.y += Sin(j.m2dAngle_) * j.m2dLl;
}
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//
// 機能 : 連続角度制限超過回数
// 戻り値 : なし
// 備考 : なし
//
void Ik2dSolver::clearExcess(void)
{
for (Ik2dJointData& j : mIkJoints)
{
j.mExcess = 0;
}
}