/// <summary>
/// 操作点が球に接触（進入）していた際に発生する力を求める
/// </summary>
/// <param name="tipPosition">操作点の座標 [mm]</param>
/// <param name="tipVelocity">操作点の速度 [mm/s]</param>
/// <returns></returns>
public Vector3D CalculateForce(Vector3D tipPosition, Vector3D tipVelocity)
{
Vector3D vec = tipPosition - this.Position;
double distance = vec.Length;
// 球の外や真ん中では力なし
if (distance >= this.Radius || distance == 0)
{
return Vector3D.Zero;
}
vec /= distance; // 正規化
double f = this.Stiffness * (this.Radius - distance);
if (f > this.ForceLimit) f = this.ForceLimit;
return (f * vec) - this.Dumping * tipVelocity;
}
/// <summary>
/// 可動範囲を取得
/// </summary>
private void LoadWorkspaceLimit()
{
double[] val = new double[6];
// 可動限界範囲を取得
Hd.hdGetDoublev(Hd.ParameterName.HD_MAX_WORKSPACE_DIMENSIONS, val);
ErrorCheck("Getting max workspace");
WorkspaceMinimum = new Vector3D(val[0], val[1], val[2]);
WorkspaceMaximum = new Vector3D(val[3], val[4], val[5]);
// 推奨可動範囲を取得
Hd.hdGetDoublev(Hd.ParameterName.HD_USABLE_WORKSPACE_DIMENSIONS, val);
ErrorCheck("Getting usable workspace");
UsableWorkspaceMinimum = new Vector3D(val[0], val[1], val[2]);
UsableWorkspaceMaximum = new Vector3D(val[3], val[4], val[5]);
// 机の高さを取得
float[] offset = new float[1];
Hd.hdGetFloatv(Hd.ParameterName.HD_TABLETOP_OFFSET, offset);
ErrorCheck("Getting table-top offset");
TableTopOffset = (double)offset[0];
}
/// <summary>
/// PHANTOM発揮力を設定します
/// </summary>
/// <param name="force">力のベクトル [N]</param>
public void SetForce(Vector3D force)
{
double[] forceArray = force.ToArray();
Hd.hdSetDoublev(Hd.ParameterName.HD_CURRENT_FORCE, forceArray);
}
/// <summary>
/// 位置と半径を指定して球を作成
/// </summary>
/// <param name="position">中心座標[mm]</param>
/// <param name="radius">半径[mm]</param>
public Orb(Vector3D position, double radius)
{
this.Position = position;
this.Radius = radius;
}
/// <summary>
/// PHANTOMで呼ばれるメソッド。
/// ここで発揮力を設定するなどの処理を行う。
/// </summary>
/// <returns>実行を繰り返したい場合はtrueを返す</returns>
private bool ServoLoop()
{
// 現在のPHANTOM情報
HandPosition = Phantom.GetPosition(); // ジンバル部座標 [mm]
HandVelocity = Phantom.GetVelocity(); // ジンバル部速度 [mm/s]
// PHANTOMで発生させる力ベクトル [N]
Vector3D force = Vector3D.Zero;
// 剛体の球体を表現
foreach (ManagedPhantom.RigidPrimitives.Orb orb in RigidOrbs)
{
force += orb.CalculateForce(HandPosition);
}
// PHANTOMの発揮力を設定
Phantom.SetForce(force);
// 終わる場合はfalse、繰り返す場合は true を返す
return true;
}
