/// <summary>
/// PPHANTOM の周期（デフォルト1kHz）で繰り返し呼ばれるメソッド
/// </summary>
/// <description>この中ではPHANTOMの座標系/単位</description>
/// <returns><c>true</c>, if update was phantomed, <c>false</c> otherwise.</returns>
bool PhantomUpdate()
{
// 手先（ジンバル部分）の座標を取得 [mm]
HandPosition = Phantom.GetPosition();
// 手先姿勢を取得 [mm]
HandRotation = Phantom.GetRotation();
// PHANTOMの発揮力 [N]
Vector3 force;
// ターゲットから引き離す方向に力をかける
// 今回、内容は適当。
// 本当は Unity の物理演算結果から力を持ってきたい
// でもやり方分らない (>_<)
Vector3 vec = TipPosition - TargetPosition;
float mag = vec.magnitude;
// 0.05
if (mag < 0.5f)
{
// 距離が 0.5 [mm] より目標に近いければ発揮力無しとする
// 大きくなりすぎるのと、向きが不安定になるため
force = Vector3.zero;
}
else
{
// 発揮力は目標に近づくと指数的に上昇
float forceMag = ForceEfficent * Mathf.Log(Radius / mag) + ForceEfficent;
if (forceMag < 0.0f)
{
forceMag = 0.0f;
}
// 安全のため最大発揮力を超える力はカット
if (forceMag > MaxForce)
{
forceMag = MaxForce;
}
// ベクトルを正規化して、求めた大きさを掛ける
force = vec * forceMag / mag;
}
// PHANTOMに対し力を指定
Phantom.SetForce(force);
return(true);
}
/// <summary>
/// PHANTOM の周期（デフォルト1kHz）で繰り返し呼ばれるメソッド
/// </summary>
/// <description>この中ではPHANTOMの座標系/単位</description>
/// <returns><c>true</c>, if update was phantomed, <c>false</c> otherwise.</returns>
bool PhantomUpdate()
{
// 手先（ジンバル部分）の座標を取得 [mm]
HandPosition = Phantom.GetPosition();
// 手先の速度を取得 [mm/s]
HandVelocity = Phantom.GetVelocity();
// 手先姿勢を取得
HandRotation = Phantom.GetRotation();
// ペン先端の座標を取得 [mm]
TipPosition = Phantom.GetTipPosition();
// PHANTOMの発揮力 [N]
Vector3 force = Vector3.zero;
// 球体から受ける力を計算
if (SphereList != null)
{
foreach (PhantomRigidSphere sphere in SphereList)
{
force += sphere.CalculateForce(TipPosition, HandVelocity);
}
}
// 力の上限を超えないようにする
if (force.sqrMagnitude > (MaxForce * MaxForce))
{
force.Normalize();
force *= MaxForce;
}
// PHANTOMに対し力を指定
Phantom.SetForce(force);
return(true);
}
