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