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>
/// グリップの半径[m]を取得する．
/// </summary>
/// <returns>
/// グリップの半径[m]
/// </returns>
public float GetGripRadius()
{
float gripRadius;
Spidar.GetGripRadius(this.SerialNumber, out gripRadius);
return(gripRadius);
}