// 可視化
public void OnDrawGizmos()
{
if (sprObject != null)
{
float length = 0.01f;
Vector3 top;
Posed socketPose = new Posed();
Posed plugPose = new Posed();
//各種Ifの取得
PHBallJointIf ball = gameObject.GetComponent <PHBallJointBehaviour>().sprObject as PHBallJointIf;
Transform jointtrans = gameObject.transform;
ball.GetSocketPose(socketPose);
socketPose = ball.GetSocketSolid().GetPose() * socketPose;
top = socketPose.Pos().ToVector3();
Vec3d torque = ball.GetMotorForceN(1);
if (torque.norm() > 100)
{
Quaternion q = ball.GetPosition().ToQuaternion();
print(gameObject.name + "torque(1):" + torque + " pos:" + q.eulerAngles);
Gizmos.color = Color.red;
Gizmos.DrawLine(top, length * (socketPose * new Vec3d(torque.x, 0, 0)).ToVector3());
Gizmos.color = Color.green;
Gizmos.DrawLine(top, length * (socketPose * new Vec3d(0, torque.y, 0)).ToVector3());
Gizmos.color = Color.blue;
Gizmos.DrawLine(top, length * (socketPose * new Vec3d(0, 0, torque.z)).ToVector3());
}
}
}
// ----- ----- ----- ----- ----- ----- ----- ----- ----- -----
// MonoBehaviourのメソッド
// Update is called once per frame
public void FixedUpdate()
{
PHBallJointIf ball = gameObject.GetComponent <PHBallJointBehaviour>().sprObject as PHBallJointIf;
Vec3d delta = ToEuler(ball.GetPosition());
Vec3d torque = ball.GetMotorForceN(1);
for (int i = 0; i < 3; i++)
{
if (values[i].springMode == 1)
{
double torqueFromCalc = Math.Exp(values[i].springParam[0] * (delta[i] - values[i].springParam[1])) - Math.Exp(values[i].springParam[2] * (values[i].springParam[3] - delta[i]));
print(gameObject.name + "ResistTorque[" + i + "] (fromMotor):" + torque[i] + " (fromCalc):" + torqueFromCalc);
}
}
}