// ----- ----- ----- ----- ----- ----- ----- ----- ----- -----
// CDShapeBehaviourの派生クラスで実装するメソッド
// -- 形状固有のShapePoseの取得。剛体からの相対位置姿勢による分は除く
public override Posed ShapePose(GameObject shapeObject)
{
CapsuleCollider cc = shapeObject.GetComponent <CapsuleCollider>();
if (cc == null)
{
throw new ObjectNotFoundException("CDCapsuleBehaviour requires CapsuleCollider", shapeObject);
}
// SpringheadとUnityでカプセルの向きが違うことに対する補正
Quaterniond diffRot = Quaternion.identity.ToQuaterniond();
// X-Axis
if (cc.direction == 0)
{
diffRot = Quaterniond.Rot(-90 * Mathf.Deg2Rad, new Vec3d(0, 1, 0));
}
// Y-Axis
if (cc.direction == 1)
{
diffRot = Quaterniond.Rot(90 * Mathf.Deg2Rad, new Vec3d(1, 0, 0));
}
// Z-Axis
if (cc.direction == 2)
{
diffRot = Quaternion.identity.ToQuaterniond();
}
return(new Posed(cc.center.ToVec3d(), diffRot));
}
public void UpdateIKTargetPosition()
{
if (iktarget != null)
{
if (sprObject != null)
{
PHIKEndEffectorIf phIKee = sprObject as PHIKEndEffectorIf;
if (phIKee.GetOriCtlMode() == PHIKEndEffectorDesc.OriCtlMode.MODE_LOOKAT)
{
phIKee.SetTargetLookat(iktarget.transform.position.ToVec3d());
}
else
{
phIKee.SetTargetPosition(iktarget.transform.position.ToVec3d());
// 現在の姿勢からの回転角がより少ない方のQuaternionに変換
Quaterniond qT = iktarget.transform.rotation.ToQuaterniond();
Quaterniond qDiff = qT * phIKee.GetSolid().GetPose().Ori().Inv();
qDiff = Quaterniond.Rot(qDiff.RotationHalf());
qT = qDiff * phIKee.GetSolid().GetPose().Ori();
phIKee.SetTargetOrientation(qT);
}
}
}
if (ikLocalTarget != null)
{
if (sprObject != null)
{
Vec3d targetLocalPos = gameObject.transform.ToPosed().Inv() * ikLocalTarget.transform.position.ToVec3d();
phIKEndEffector.SetTargetLocalPosition(targetLocalPos);
}
}
}
// 可視化
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);
}
}
// ----- ----- ----- ----- ----- ----- ----- ----- ----- -----
// CDShapeBehaviourの派生クラスで実装するメソッド
// -- 形状固有のShapePoseの取得。剛体からの相対位置姿勢による分は除く
public override Posed ShapePose(GameObject shapeObject)
{
MeshRoundCone mrc = shapeObject.GetComponent <MeshRoundCone>();
if (mrc == null)
{
throw new ObjectNotFoundException("CDRoundConeBehaviour requires MeshRoundCone", shapeObject);
}
// SpringheadとUnityでカプセルの向きが違うことに対する補正
Vec3f p = new Vec3f();
if (mrc.pivot == MeshRoundCone.Pivot.R1)
{
p = new Vec3f(+0.5f * mrc.length, 0, 0);
}
else if (mrc.pivot == MeshRoundCone.Pivot.R2)
{
p = new Vec3f(-0.5f * mrc.length, 0, 0);
}
return(new Posed(p, Quaterniond.Rot(90.0f * Mathf.Deg2Rad, new Vec3d(0, 1, 0))));
}
// 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();
}
}
}
// ----- ----- ----- ----- ----- ----- ----- ----- ----- -----
// CDShapeBehaviourの派生クラスで実装するメソッド
// -- 形状固有のShapePoseの取得。剛体からの相対位置姿勢による分は除く
public override Posed ShapePose(GameObject shapeObject)
{
// SpringheadとUnityでカプセルの向きが違うことに対する補正
return(new Posed(new Vec3d(), Quaterniond.Rot(90 * Mathf.Deg2Rad, new Vec3d(1, 0, 0))));
}
