// 可視化
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);
}
}
// ----- ----- ----- ----- ----- ----- ----- ----- ----- -----
// MonoBehaviourのメソッド
void FixedUpdate()
{
if (sprObject != null)
{
hiSpidar.Update(Time.fixedDeltaTime);
if (lengthText != null)
{
string text = "";
for (int i = 0; i < hiSpidar.NMotor(); i++)
{
text += hiSpidar.GetMotor((uint)i).GetLength().ToString() + "\r\n";
}
lengthText.text = text;
}
if (pointer != null)
{
Posed pose = hiSpidar.GetPose();
pointer.transform.position = pose.Pos().ToVector3();
pointer.transform.rotation = pose.Ori().ToQuaternion();
}
}
}
// ----- ----- ----- ----- -----
// Spr --> Unity
public static void FromPosed(this Transform t, Posed pose)
{
t.position = pose.Pos().ToVector3();
t.rotation = pose.Ori().ToQuaternion();
}
void OnSceneGUI()
{
PH1DJointLimitBehavior limit = (PH1DJointLimitBehavior)target;
Vec2d range = limit.desc.range;
GameObject jointObject = limit.jointObject ? limit.jointObject : limit.gameObject;
// Hinge
PHHingeJointBehaviour phHingeJointBehaviour = jointObject.GetComponent <PHHingeJointBehaviour>();
if (phHingeJointBehaviour)
{
GameObject jointPositionObject = phHingeJointBehaviour.jointObject ? phHingeJointBehaviour.jointObject : phHingeJointBehaviour.gameObject;
Transform jointTransform = jointPositionObject.transform;
Posed plugPose = phHingeJointBehaviour.plugPose;
Posed socketPose = phHingeJointBehaviour.socketPose;
Vector3 plugPosition = plugPose.Pos().ToVector3();
Vector3 socketPosition = socketPose.Pos().ToVector3();
Quaternion plugRot = plugPose.Ori().ToQuaternion();
Quaternion socketRot = socketPose.Ori().ToQuaternion();
Color baseColor;
if (limit.desc.bEnabled && limit.enabled && limit.desc.range.x < limit.desc.range.y)
{
if (limit.phJointLimit == null)
{
baseColor = Color.green;
}
else
{
if (limit.phJointLimit.IsOnLimit())
{
baseColor = Color.red;
}
else
{
baseColor = Color.green;
}
}
baseColor.a = 0.3f;
}
else
{
baseColor = Color.white;
}
EditorGUI.BeginChangeCheck();
Vector3 jointPosition = socketPosition;
Vector3 jointAxis = socketRot * new Vector3(0, 0, 1);
Handles.color = baseColor;
Vector3 rangeXHandlePos = jointPosition + Quaternion.AngleAxis((float)(range[0] * Mathf.Rad2Deg), socketRot * new Vector3(0, 0, 1)) * (socketRot * (discRadius * handDir));
Vector3 rangeXHandleDir = Quaternion.AngleAxis((float)(range[0] * Mathf.Rad2Deg) + 90, socketRot * new Vector3(0, 0, 1)) * (socketRot * (discRadius * handDir));
Vector3 rangeYHandlePos = jointPosition + Quaternion.AngleAxis((float)(range[1] * Mathf.Rad2Deg), socketRot * new Vector3(0, 0, 1)) * (socketRot * (discRadius * handDir));
Vector3 rangeYHandleDir = Quaternion.AngleAxis((float)(range[1] * Mathf.Rad2Deg) + 90, socketRot * new Vector3(0, 0, 1)) * (socketRot * (discRadius * handDir));
Handles.DrawWireDisc(jointPosition, socketRot * new Vector3(0, 0, 1), discRadius);
Handles.DrawSolidArc(jointPosition, jointAxis, rangeXHandlePos - jointPosition, (float)(range[1] - range[0]) * Mathf.Rad2Deg, discRadius);
Vector3 rangeX = Handles.Slider2D(rangeXHandlePos, jointAxis, rangeXHandlePos - jointPosition, rangeXHandleDir, 0.01f, Handles.CubeHandleCap, 0f);
Vector3 rangeY = Handles.Slider2D(rangeYHandlePos, jointAxis, rangeYHandlePos - jointPosition, rangeYHandleDir, 0.01f, Handles.CubeHandleCap, 0f);
Handles.color = Color.yellow;
Vector3 currentHand = plugPosition + (plugRot * ((discRadius + 0.01f) * handDir));
Handles.DrawLine(plugPosition, currentHand);
if (EditorGUI.EndChangeCheck())
{
Undo.RecordObject(limit, "Undo Limit Chnage");
float deltaRangeX = Vector3.SignedAngle(rangeXHandlePos - jointPosition, rangeX - jointPosition, jointAxis) * Mathf.Deg2Rad;
float deltaRangeY = Vector3.SignedAngle(rangeYHandlePos - jointPosition, rangeY - jointPosition, jointAxis) * Mathf.Deg2Rad;
limit.desc.range = new Vec2d(deltaRangeX + range[0], deltaRangeY + range[1]);
limit.OnValidate();
}
}
// Slider
}
// JointのautoSetSockPlugPoseがonだという前提
void OnSceneGUI()
{
PHBallJointLimitBehavior limit = (PHBallJointLimitBehavior)target;
GameObject jointObject = limit.jointObject ? limit.jointObject : limit.gameObject;
PHBallJointBehaviour phBallJointBehaviour = jointObject.GetComponent <PHBallJointBehaviour>();
if (phBallJointBehaviour)
{
GameObject jointPositionObject = phBallJointBehaviour.jointObject ? phBallJointBehaviour.jointObject : phBallJointBehaviour.gameObject;
Transform jointTransform = jointPositionObject.transform;
Posed plugPose = phBallJointBehaviour.plugPose;
Posed socketPose = phBallJointBehaviour.socketPose;
Vector3 jointPosition = socketPose.Pos().ToVector3();
Vector3 twistAxis = plugPose.Ori().ToQuaternion() * new Vector3(0, 0, 1);
//Limit情報の取得
//Vec2d swing = new Vec2d(); limit.phJointLimit.GetSwingRange(swing);
Vec2d swing = limit.desc.limitSwing;
//Vec2d twist = new Vec2d(); limit.phJointLimit.GetTwistRange(twist);
Vec2d twist = limit.desc.limitTwist;
Vec2i twistRap = new Vec2i((int)((twist[0] + Mathf.PI) / (2 * Mathf.PI)), (int)((twist[0] + Mathf.PI) / (2 * Mathf.PI)));
Vec3d limitDir = limit.desc.limitDir;
double currTwistAngle = (phBallJointBehaviour.sprObject != null) ? (phBallJointBehaviour.phBallJoint.GetAngle())[2] : 0;
Color baseColor;
if (limit.desc.bEnabled && limit.enabled && twist[0] < twist[1])
{
if (limit.phJointLimit == null)
{
baseColor = Color.green;
}
else
{
if (limit.phJointLimit.IsOnLimit())
{
baseColor = Color.red;
}
else
{
baseColor = Color.green;
}
}
baseColor.a = 0.3f;
}
else
{
baseColor = Color.white;
}
// 編集開始
EditorGUI.BeginChangeCheck();
// Swing
// 可視化
Vec3d swingBaseAxis = socketPose * (length * limitDir);
Quaterniond Jztol = Quaternion.FromToRotation(new Vector3(0, 0, 1), limitDir.ToVector3()).ToQuaterniond();
Vec3d swingDirBase1 = (length * (limitDir * Mathf.Cos((float)swing[0]) + Jztol * new Vec3d(1, 0, 0) * Mathf.Sin((float)swing[0])));
Vec3d swingDirBase2 = (length * (limitDir * Mathf.Cos((float)swing[1]) + Jztol * new Vec3d(1, 0, 0) * Mathf.Sin((float)swing[1])));
if (swing[0] >= swing[1])
{
Handles.color = Color.white;
}
else
{
Handles.color = baseColor;
}
Quaterniond rot = Quaterniond.Rot(2 * Mathf.PI / numDivision, limitDir);
for (int i = 0; i < numDivision; i++)
{
Handles.DrawLine((socketPose * swingDirBase1).ToVector3(), jointPosition);
Handles.DrawLine((socketPose * swingDirBase2).ToVector3(), jointPosition);
//Handles.DrawLine((socketPose * swingDirBase1).ToVector3(), (socketPose * swingDirBase2).ToVector3());
Vector3 arcNormal = Vector3.Cross(((socketPose * swingDirBase1).ToVector3() - jointPosition), ((socketPose * swingDirBase2).ToVector3() - jointPosition));
//Handles.DrawWireArc(jointPosition, arcNormal, (socketPose * swingDirBase1).ToVector3() - jointPosition, (float)(swing[1] - swing[0]) * Mathf.Rad2Deg, length);
swingDirBase1 = rot * swingDirBase1;
swingDirBase2 = rot * swingDirBase2;
}
Vec3d swingCircleCenter1 = socketPose * (length * Mathf.Cos((float)swing[0]) * limitDir);
Vec3d swingCircleCenter2 = socketPose * (length * Mathf.Cos((float)swing[1]) * limitDir);
Handles.DrawWireDisc(swingCircleCenter1.ToVector3(), swingBaseAxis.ToVector3() - jointPosition, length * Mathf.Abs(Mathf.Sin((float)swing[0])));
Handles.DrawWireDisc(swingCircleCenter2.ToVector3(), swingBaseAxis.ToVector3() - jointPosition, length * Mathf.Abs(Mathf.Sin((float)swing[1])));
Handles.DrawLine(jointPosition, swingBaseAxis.ToVector3());
// ハンドル
Vector3 SwingXHandlePos = (socketPose * (length * Mathf.Cos((float)swing[0]) * limitDir)).ToVector3();
Vector3 SwingX = Handles.Slider(SwingXHandlePos, (socketPose * limitDir).ToVector3() - SwingXHandlePos, 0.01f, Handles.ArrowHandleCap, 0f);
Vector3 SwingYHandlePos = (socketPose * (length * Mathf.Cos((float)swing[1]) * limitDir)).ToVector3();
Vector3 SwingY = Handles.Slider(SwingYHandlePos, (socketPose * limitDir).ToVector3() - SwingYHandlePos, 0.01f, Handles.ArrowHandleCap, 0f);
// Twist
// とりあえずplugのx軸を基準とする
// 可視化
Vec3d currentTwistAxis = plugPose * new Vec3d(0, 0, lengthTwist);
Handles.DrawLine(plugPose.Pos().ToVector3(), currentTwistAxis.ToVector3());
// ハンドル
Vector3 discCenter = (plugPose * new Vec3d(0, 0, lengthTwist)).ToVector3();
Vector3 discNormal = ((plugPose * new Vec3d(0, 0, lengthTwist)) - plugPose.Pos()).ToVector3();
Vector3 currentTwist = ((plugPose * new Vec3d((discRadius + 0.01f) * Mathf.Cos((float)currTwistAngle), (discRadius + 0.01f) * Mathf.Sin((float)currTwistAngle), lengthTwist)).ToVector3());
Vector3 twistXHandlePos = (plugPose * (new Vec3d(0.03 * Mathf.Cos((float)twist[0]), 0.03 * Mathf.Sin((float)twist[0]), lengthTwist))).ToVector3();
Vector3 twistXHandleDir = (plugPose * (new Vec3d(0.03 * -Mathf.Sin((float)twist[0]), 0.03 * Mathf.Cos((float)twist[0]), 0)) - plugPose.Pos()).ToVector3();
Vector3 twistYHandlePos = (plugPose * (new Vec3d(0.03 * Mathf.Cos((float)twist[1]), 0.03 * Mathf.Sin((float)twist[1]), lengthTwist))).ToVector3();
Vector3 twistYHandleDir = (plugPose * (new Vec3d(0.03 * -Mathf.Sin((float)twist[1]), 0.03 * Mathf.Cos((float)twist[1]), 0)) - plugPose.Pos()).ToVector3();
Handles.DrawWireDisc(discCenter, discNormal, discRadius);
Handles.DrawSolidArc(discCenter, discNormal, twistXHandlePos - discCenter, (float)(twist[1] - twist[0]) * Mathf.Rad2Deg, discRadius);
Vector3 twistX = Handles.Slider2D(twistXHandlePos, twistAxis, twistXHandlePos - discCenter, twistXHandleDir, 0.01f, Handles.CubeHandleCap, 0f);
Vector3 twistY = Handles.Slider2D(twistYHandlePos, twistAxis, twistYHandlePos - discCenter, twistYHandleDir, 0.01f, Handles.CubeHandleCap, 0f);
Handles.color = Color.yellow;
Handles.DrawLine(discCenter, currentTwist);
if (twist[0] >= twist[1])
{
Handles.color = Color.white;
}
else
{
Handles.color = baseColor;
}
if (EditorGUI.EndChangeCheck())
{
Undo.RecordObject(limit, "Undo Limit Chnage");
// Swing
limit.desc.limitSwing = new Vec2d(Mathf.Acos((Vector3.Dot((SwingX - socketPose.Pos().ToVector3()), (swingBaseAxis.ToVector3() - socketPose.Pos().ToVector3())) > 0 ? 1 : -1) * Mathf.Max(Mathf.Min((SwingX - socketPose.Pos().ToVector3()).magnitude / length, 1), -1)),
Mathf.Acos((Vector3.Dot((SwingY - socketPose.Pos().ToVector3()), (swingBaseAxis.ToVector3() - socketPose.Pos().ToVector3())) > 0 ? 1 : -1) * Mathf.Max(Mathf.Min((SwingY - socketPose.Pos().ToVector3()).magnitude / length, 1), -1)));
// Twist
float deltaTwistX = Vector3.SignedAngle(twistXHandlePos - discCenter, twistX - discCenter, discCenter - plugPose.Pos().ToVector3()) * Mathf.Deg2Rad;
float deltaTwistY = Vector3.SignedAngle(twistYHandlePos - discCenter, twistY - discCenter, discCenter - plugPose.Pos().ToVector3()) * Mathf.Deg2Rad;
limit.desc.limitTwist = new Vec2d(deltaTwistX + twist[0], deltaTwistY + twist[1]);
limit.OnValidate();
}
}
}