// 可視化 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); } } }