void SetSpidarForce()
{
if (spidar == null)
{
return;
}
spidar.SetHaptics(Haptics);
spidar.SetCascadeGain(CascadeGain);
if (holdingObject == null || !clutchEngaged)
{
return;
}
HoldState hs = GetHoldState();
Vector3 g = Vector3.zero;
if (Gravity)
{
g = Vector3.down * holdingObject.mass * 9.81f;
g = Quaternion.Inverse(RotationOffset) * g;
}
if (!hs.Collision && !curMultiHold)
{
if (Gravity)
{
spidar.SetForce(Converter.Convert(g), 0, 0, Converter.Convert(Vector3.zero), 0, 0, true, false);
}
else
{
spidar.ClearForce(true);
}
return;
}
float deviceR2 = spidar.GetGripRadius() * spidar.GetGripRadius();
float forceScale = DeviceSpringK / (UnitySpringK * PositionScale);
float torqueScale = DeviceSpringK / (UnitySpringK * RotationScale);
Vector3 f = -model.CalcForce(pose, holdingObject) * forceScale;
Vector3 t = -model.CalcTorque(pose, holdingObject) * torqueScale * deviceR2;
f = Quaternion.Inverse(RotationOffset) * f;
t = Quaternion.Inverse(RotationOffset) * t;
float forceK = DeviceSpringK;
float forceB = DeviceDamperB;
float torqueK = DeviceSpringK * deviceR2;
float torqueB = DeviceDamperB * deviceR2;
spidar.SetForce(Converter.Convert(f + g), forceK, forceB, Converter.Convert(t), torqueK, torqueB, false, CascadeControl);
}
/// <summary>
/// 提示力を設定する．
/// </summary>
/// <param name="force">
/// 出力する併進力[N]（目標値）
/// </param>
/// <param name="torque">
/// 出力する回転力[Nm]（目標値）
/// </param>
/// <param name="forceK">
/// バネ定数 K
/// </param>
/// <param name="forceB">
/// ダンパ係数 B
/// </param>
/// <param name="torqueK">
/// バネ定数 K
/// </param>
/// <param name="torqueB">
/// ダンパ係数 B
/// </param>
/// <param name="lerp">
/// 提示力の線形補間のON/OFF
/// </param>
/// <param name="cascade">
/// カスケード制御のON/OFF
/// </param>
public void SetForce(Vector3 force, float forceK, float forceB,
Vector3 torque, float torqueK, float torqueB, bool lerp, bool cascade)
{
Spidar.SetForce(this.SerialNumber, ref force, forceK, forceB,
ref torque, torqueK, torqueB,
lerp, cascade);
}
/// <summary>
/// 力提示をやめ最小張力とする．
/// </summary>
/// <param name="lerp">
/// 提示力補間のON/OFF
/// </param>
public void ClearForce(bool lerp = false)
{
Vector3 zero = Vector3.zero;
Spidar.SetForce(this.SerialNumber, ref zero, 0, 0, ref zero, 0, 0, lerp, false);
}