// 可視化
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());
}
}
}
// 可視化
public void OnDrawGizmos()
{
//可動範囲(SwingDirとTwistは無視)の表示
if (sprObject != null)
{
float length = 0.1f;
int n = 30;
Vector3 top;
Posed socketPose = new Posed();
Posed plugPose = new Posed();
//各種Ifの取得
PHBallJointIf ball = (jointObject ? jointObject : gameObject).GetComponent <PHBallJointBehaviour>().sprObject as PHBallJointIf;
PHBallJointConeLimitIf limit = sprObject as PHBallJointConeLimitIf;
//Limit情報の取得
Vec2d swing = new Vec2d(); limit.GetSwingRange(swing);
Vec3d limitDir = limit.GetLimitDir();
//可動範囲円の計算表示
Transform jointtrans = (jointObject ? jointObject : gameObject).transform;
ball.GetSocketPose(socketPose);
socketPose = ball.GetSocketSolid().GetPose() * socketPose;
top = socketPose.Pos().ToVector3();
Vec3d bottom = socketPose * (length * limit.GetLimitDir());
Quaterniond Jztol = Quaternion.FromToRotation(new Vector3(0, 0, 1), limit.GetLimitDir().ToVector3()).ToQuaterniond();
Vec3d swingDirBase1 = (length * (limitDir * Mathf.Cos((float)swing[1]) + Jztol * new Vec3d(1, 0, 0) * Mathf.Sin((float)swing[1])));
Vec3d swingDirBase2 = (length * (limitDir * Mathf.Cos((float)swing[0]) + Jztol * new Vec3d(1, 0, 0) * Mathf.Sin((float)swing[0])));
if (limit.IsOnLimit())
{
Gizmos.color = Color.red;
}
else
{
Gizmos.color = Color.white;
}
Quaterniond rot = Quaterniond.Rot(2 * Mathf.PI / n, limitDir);
for (int i = 0; i < n; i++)
{
Gizmos.DrawLine((socketPose * swingDirBase1).ToVector3(), (socketPose * (rot * swingDirBase1)).ToVector3());
Gizmos.DrawLine((socketPose * swingDirBase2).ToVector3(), (socketPose * (rot * swingDirBase2)).ToVector3());
Gizmos.DrawLine((socketPose * swingDirBase1).ToVector3(), (socketPose * swingDirBase2).ToVector3());
swingDirBase1 = rot * swingDirBase1;
swingDirBase2 = rot * swingDirBase2;
}
//Swing軸の表示
ball.GetPlugPose(plugPose);
plugPose = ball.GetPlugSolid().GetPose() * plugPose;
Vec3d axis = plugPose * (length * new Vec3d(0, 0, 1));
Gizmos.DrawLine(top, axis.ToVector3());
//Gizmos.DrawLine(top, bottom);
}
}