// -- Sprオブジェクトの構築を行う public override ObjectIf Build() { PHBallJointIf jo = gameObject.GetComponent <PHBallJointBehaviour>().sprObject as PHBallJointIf; if (jo == null) { return(null); } PHBallJointNonLinearMotorDesc d = new PHBallJointNonLinearMotorDesc(); PHBallJointNonLinearMotorIf motor = jo.CreateMotor(PHBallJointNonLinearMotorIf.GetIfInfoStatic(), d) as PHBallJointNonLinearMotorIf; if (motor == null) { return(null); } arraywrapper_double s_double, d_double; for (int i = 0; i < 3; i++) { switch (values[i].springMode) { case 0: s_double = new arraywrapper_double(2); Copy(values[i].springParam, s_double, 2); break; case 1: s_double = new arraywrapper_double(4); Copy(values[i].springParam, s_double, 4); break; default: values[i].springMode = 0; s_double = new arraywrapper_double(4); Copy(values[i].springParam, s_double, 4); break; } switch (values[i].damperMode) { case 0: d_double = new arraywrapper_double(2); Copy(values[i].damperParam, d_double, 2); break; case 1: d_double = new arraywrapper_double(4); Copy(values[i].damperParam, d_double, 4); break; default: values[i].damperMode = 0; d_double = new arraywrapper_double(4); Copy(values[i].damperParam, d_double, 4); break; } motor.SetFuncFromDatabaseN(i, values[i].springMode, values[i].damperMode, s_double, d_double); print(i + " " + values[i].springMode + " " + values[i].damperMode + " " + s_double + " " + d_double); } return(motor); }
// 可視化 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); } }
// -- Sprオブジェクトの構築を行う public override ObjectIf Build() { PHBallJointIf jo = gameObject.GetComponent <PHBallJointBehaviour>().sprObject as PHBallJointIf; if (jo == null) { return(null); } PHHumanBallJointResistanceIf motor = jo.CreateMotor(PHHumanBallJointResistanceIf.GetIfInfoStatic(), (PHHumanBallJointResistanceDesc)desc) as PHHumanBallJointResistanceIf; return(motor); }
// ----- ----- ----- ----- ----- ----- ----- ----- ----- ----- // 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); } } }
// -- Sprオブジェクトの構築を行う public override ObjectIf Build() { PHBallJointIf jo = null; var b = (jointObject ? jointObject : gameObject).GetComponent <PHBallJointBehaviour>(); if (!b) { return(null); } jo = b.sprObject as PHBallJointIf; if (jo == null) { return(null); } PHBallJointLimitIf lim = jo.CreateLimit(PHBallJointConeLimitIf.GetIfInfoStatic(), (PHBallJointConeLimitDesc)desc); return(lim); }
static void test_simulation() { test_name("physical simulation"); PHSceneDesc descScene = new PHSceneDesc(); PHSolidDesc descSolid = new PHSolidDesc(); CDBoxDesc descBox = new CDBoxDesc(); PHSdkIf phSdk = PHSdkIf.CreateSdk(); PHSceneIf phScene = phSdk.CreateScene(descScene); PHSolidIf phSolid = phScene.CreateSolid(descSolid); phSolid.AddShape(phSdk.CreateShape(CDBoxIf.GetIfInfoStatic(), descBox)); phSolid.SetPose(new Posed(1, 0, 0, 0, 0, 2, 0)); PHSolidIf phFloor = phScene.CreateSolid(descSolid); phFloor.SetDynamical(false); descBox.boxsize = new Vec3f(10, 10, 10); phFloor.AddShape(phSdk.CreateShape(CDBoxIf.GetIfInfoStatic(), descBox)); phFloor.SetPose(new Posed(1, 0, 0, 0, 0, -5, 0)); PHBallJointDesc descJoint = new PHBallJointDesc(); PHBallJointIf j = phScene.CreateJoint(phFloor, phSolid, PHBallJointIf.GetIfInfoStatic(), descJoint).Cast(); System.Console.WriteLine(j.GetName()); PHIKBallActuatorDescStruct s = new PHIKBallActuatorDesc(); put("bias", "something", s.bias); /**/ for (int i = 0; i < 200; i++) { phScene.Step(); //System.Console.WriteLine(i.ToString() + " : " + phSolid.GetPose()); System.Console.WriteLine(String.Format("{0, 3}", i) + " : " + phSolid.GetPose()); } /**/ }
// ----- ----- ----- ----- ----- ----- ----- ----- ----- ----- // PHJointBehaviourの派生クラスで実装するメソッド // -- 関節を作成する public override PHJointIf CreateJoint(PHSolidIf soSock, PHSolidIf soPlug) { return(phScene.CreateJoint(soSock, soPlug, PHBallJointIf.GetIfInfoStatic(), (PHBallJointDesc)desc)); }