// 可視化
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());
}
}
}
static void test_intrinsic()
{
test_name("intrinsic");
// intrinsic member
PHSceneDesc descScene = new PHSceneDesc();
PHRaycastHit raycastHit = new PHRaycastHit();
GRVertexElement vertexelm = new GRVertexElement();
// simple
vertexelm.offset = 123; put("short ", "123 ", vertexelm.offset);
descScene.numIteration = 123; put("int ", "123 ", descScene.numIteration);
descScene.bCCDEnabled = true; put("bool ", "True ", descScene.bCCDEnabled);
raycastHit.distance = 0.123F; put("float ", "0.123", raycastHit.distance);
descScene.airResistanceRate = 0.123; put("double", "0.123", descScene.airResistanceRate);
// nested
descScene.gravity.x = 0;
descScene.gravity.y = 0;
descScene.gravity.z = -4.5;
/**/ put("set by elm", "(0.0, 0.0, -4.5)", descScene.gravity);
descScene.gravity = new Vec3d(2.5, -5.2, 0.5);
/**/ put("set struct", "(2.5, -5.2, 0.5)", descScene.gravity);
// structure
Vec3d v3d = new Vec3d(0.1, 0.2, 0.3);
put("Vec new", "(0.1, 0.2, 0.3)", v3d);
put("Vec * c", "(0.2, 0.4, 0.6)", v3d * 2);
put("c * Vec", "(0.2, 0.4, 0.6)", 2 * v3d);
Posed pose = new Posed(new Vec3d(1, 2, 3), new Quaterniond(1, 0, 0, 0));
put2("pose", new Posed(1, 0, 0, 0, 1, 2, 3), pose);
}
// ----- ----- ----- ----- ----- ----- ----- ----- ----- -----
// CDShapeBehaviourの派生クラスで実装するメソッド
// -- 形状固有のShapePoseの取得。剛体からの相対位置姿勢による分は除く
public override Posed ShapePose(GameObject shapeObject)
{
BoxCollider bc = shapeObject.GetComponent <BoxCollider>();
Posed pose = new Posed();
pose.px = bc.center.x;
pose.py = bc.center.y;
pose.pz = bc.center.z;
return(pose);
}
// 可視化
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);
}
}
// ----- ----- ----- ----- ----- ----- ----- ----- ----- -----
// SprBehaviourの派生クラスで実装するメソッド
// -- Sprオブジェクトの構築を行う
public override ObjectIf Build()
{
if (!socket)
{
socket = gameObject.transform.parent.GetComponentInParent <PHSolidBehaviour>().gameObject;
}
if (!plug)
{
plug = gameObject.GetComponentInParent <PHSolidBehaviour>().gameObject;
}
if (socket == null)
{
throw new ObjectNotFoundException("Socket object did not found for Joint", gameObject);
}
if (plug == null)
{
throw new ObjectNotFoundException("Plug object did not found for Joint", gameObject);
}
PHSolidIf soSock = socket.GetComponent <PHSolidBehaviour>().sprObject as PHSolidIf;
PHSolidIf soPlug = plug.GetComponent <PHSolidBehaviour>().sprObject as PHSolidIf;
PHJointIf jo = CreateJoint(soSock, soPlug);
jo.SetName("jo:" + gameObject.name);
if (autoSetSockPlugPose)
{
// priority jointObject > jointPosition/Orientation > gameObject
Posed jointPose = new Posed();
if (jointObject == null)
{
jointObject = gameObject;
jointPose = jointObject.transform.ToPosed() * new Posed(jointPosition.ToVec3d(), jointOrientation.ToQuaterniond());
}
else
{
jointPose = jointObject.transform.ToPosed();
}
jo.SetSocketPose(soSock.GetPose().Inv() * jointPose);
jo.SetPlugPose(soPlug.GetPose().Inv() * jointPose);
}
return(jo);
}
// ----- ----- ----- ----- ----- ----- ----- ----- ----- -----
// SprBehaviourの派生クラスで実装するメソッド
// -- Sprオブジェクトの構築を行う
public override ObjectIf Build()
{
if (shapeObject == null)
{
shapeObject = gameObject;
}
CDShapeIf shape = CreateShape(shapeObject);
phSolid.AddShape(shape);
GameObject solidObject = solidBehaviour.gameObject;
// 剛体オブジェクトからの相対変換
Posed relPoseFromSolid = solidObject.transform.ToPosed().Inv() * shapeObject.transform.ToPosed();
phSolid.SetShapePose(phSolid.NShape() - 1, relPoseFromSolid * ShapePose(shapeObject));
return(shape);
}
public void OnSceneGUI()
{
PHJointBehaviour phJointBehaviour = (PHJointBehaviour)target;
// ----- ----- ----- ----- -----
// Target Position Handle
if (phJointBehaviour.showJointPoseHandle)
{
Tools.current = Tool.None;
// <!!> この方式だと微小変化が常に発生してあまりUndoなどが機能しない
Posed objectPose = phJointBehaviour.gameObject.transform.ToPosed();
Vector3 currJointPos = (objectPose * phJointBehaviour.jointPosition.ToVec3d()).ToVector3();
Quaternion currJointRot = phJointBehaviour.gameObject.transform.rotation * phJointBehaviour.jointOrientation;
EditorGUI.BeginChangeCheck();
Vector3 handlePos = Handles.PositionHandle(currJointPos, phJointBehaviour.gameObject.transform.rotation);
Quaternion handleRot = Handles.RotationHandle(currJointRot, currJointPos);
if (EditorGUI.EndChangeCheck())
{
Undo.RecordObject(phJointBehaviour, "Joint Pos/Rot Change");
phJointBehaviour.jointPosition = Quaternion.Inverse(phJointBehaviour.gameObject.transform.rotation) * (handlePos - phJointBehaviour.gameObject.transform.position);
phJointBehaviour.jointOrientation = Quaternion.Inverse(phJointBehaviour.gameObject.transform.rotation) * handleRot;
}
}
}
// ----- ----- ----- ----- ----- ----- ----- ----- ----- -----
// 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();
}
}
}
