protected override void ImportJointData(Joint joint)
{
ConfigurableJoint configurableJoint = (ConfigurableJoint)UnityJoint;
Vector3 normal = (joint.axis != null) ? GetAxis(joint.axis) : GetDefaultAxis();
Vector3 axisX = Vector3.forward;
Vector3 axisY = Vector3.left;
Vector3.OrthoNormalize(ref normal, ref axisX, ref axisY);
configurableJoint.axis = axisX;
configurableJoint.secondaryAxis = axisY;
// spring, damper & max. force:
if (joint.dynamics != null)
{
configurableJoint.xDrive = GetJointDrive(joint.dynamics);
configurableJoint.yDrive = GetJointDrive(joint.dynamics);
}
if (joint.limit != null)
{
configurableJoint.linearLimit = GetLinearLimit(joint.limit);
}
}
ConfigurableJoint addStrut(Part p, Part pp, Vector3 pos)
{
ConfigurableJoint configurableJoint = null;
if (p != pp && pp.Rigidbody is Rigidbody rigidbody && rigidbody != p.Rigidbody)
{
configurableJoint = p.gameObject.AddComponent <ConfigurableJoint>();
configurableJoint.xMotion = 0;
configurableJoint.yMotion = 0;
configurableJoint.zMotion = 0;
configurableJoint.angularXMotion = 0;
configurableJoint.angularYMotion = 0;
configurableJoint.angularZMotion = 0;
configurableJoint.projectionDistance = 0.1f;
configurableJoint.projectionAngle = 5f;
configurableJoint.breakForce = breakingForce;
configurableJoint.breakTorque = breakingForce;
configurableJoint.connectedBody = rigidbody;
configurableJoint.targetPosition = pos;
}
return(configurableJoint);
}
void RagdollRecursively(GameObject obj, int newLayer)
{
if (null == obj)
{
return;
}
obj.layer = newLayer;
ConfigurableJoint joint = obj.GetComponent <ConfigurableJoint>();
if (joint != null)
{
JointDrive springX = joint.angularXDrive;
JointDrive springYZ = joint.angularYZDrive;
springX.positionSpring = 0;
springYZ.positionSpring = 0;
joint.angularXDrive = springX;
joint.angularYZDrive = springYZ;
}
Rigidbody rb = obj.GetComponent <Rigidbody>();
if (rb != null)
{
rb.constraints = RigidbodyConstraints.None;
}
foreach (Transform child in obj.transform)
{
if (null == child)
{
continue;
}
RagdollRecursively(child.gameObject, newLayer);
}
}
void Start()
{
joint = GetComponent <Rigidbody>();
local = joint.transform;
configJoint = GetComponent <ConfigurableJoint>();
localPos = local.localPosition;
//if (configJoint != null)
//{
// configJoint.angularXMotion = ConfigurableJointMotion.Locked;
// configJoint.angularYMotion = ConfigurableJointMotion.Locked;
// configJoint.angularZMotion = ConfigurableJointMotion.Locked;
// configJoint.xMotion = ConfigurableJointMotion.Locked;
// configJoint.yMotion = ConfigurableJointMotion.Locked;
// configJoint.zMotion = ConfigurableJointMotion.Locked;
// if (configJoint.axis.x != 0)
// {
// joint.constraints = RigidbodyConstraints.None;
// joint.constraints = RigidbodyConstraints.FreezeRotationX;
// }
// else if (configJoint.axis.y != 0)
// {
// joint.constraints = RigidbodyConstraints.None;
// joint.constraints = RigidbodyConstraints.FreezeRotationY;
// }
// else if (configJoint.axis.z != 0)
// {
// joint.constraints = RigidbodyConstraints.None;
// joint.constraints = RigidbodyConstraints.FreezeRotationZ;
// }
//}
//local.localRotation = Quaternion.Euler(Vector3.zero);
}
/// <inheritdoc/>
public string DumpJoint(ConfigurableJoint joint)
{
if (joint == null)
{
return("<NULL JOINT>");
}
var msg = DumpBaseJoint(joint);
// Geometry.
msg.Append("secondaryAxis: ").Append(DbgFormatter.Vector(joint.secondaryAxis)).AppendLine();
// X axis settings.
msg.Append("xDrive: ").Append(Dump(joint.xDrive)).AppendLine();
msg.Append("xMotion: ").Append(joint.xMotion).AppendLine();
msg.Append("angularXMotion: ").Append(joint.angularXMotion).AppendLine();
msg.Append("angularXLimitSpring: ").Append(Dump(joint.angularXLimitSpring)).AppendLine();
msg.Append("angularXDrive: ").Append(Dump(joint.angularXDrive)).AppendLine();
msg.Append("lowAngularXLimit: ").Append(Dump(joint.lowAngularXLimit)).AppendLine();
msg.Append("highAngularXLimit: ").Append(Dump(joint.highAngularXLimit)).AppendLine();
// Y axis settings.
msg.Append("yDrive: ").Append(Dump(joint.yDrive)).AppendLine();
msg.Append("yMotion: ").Append(joint.yMotion).AppendLine();
msg.Append("angularYMotion: ").Append(joint.angularYMotion).AppendLine();
msg.Append("angularYLimit: ").Append(Dump(joint.angularYLimit)).AppendLine();
// Z axis settings.
msg.Append("zDrive: ").Append(Dump(joint.zDrive)).AppendLine();
msg.Append("zMotion: ").Append(joint.zMotion).AppendLine();
msg.Append("angularZMotion: ").Append(joint.angularZMotion).AppendLine();
msg.Append("angularZLimit: ").Append(Dump(joint.angularZLimit)).AppendLine();
// Multiple axis settings.
msg.Append("linearLimit: ").Append(Dump(joint.linearLimit)).AppendLine();
msg.Append("linearLimitSpring: ").Append(Dump(joint.linearLimitSpring)).AppendLine();
msg.Append("angularYZDrive: ").Append(Dump(joint.angularYZDrive)).AppendLine();
msg.Append("angularYZLimitSpring: ").Append(Dump(joint.angularYZLimitSpring)).AppendLine();
return(msg.ToString());
}
void GenerateRope()
{
GameObject prevRopeSegment = ropeSegment;
for (int i = 0; i < ropeLength; i++)
{
allSegments[i] = Instantiate(ropeSegment, this.transform);
ConfigurableJoint newSpring = allSegments[i].GetComponent <ConfigurableJoint>();
if (prevRopeSegment != null)
{
allSegments[i].transform.position = prevRopeSegment.transform.position + transform.right + offset;
newSpring.connectedBody = prevRopeSegment.GetComponent <Rigidbody>();
}
else
{
Debug.Log("No previous rope segment found!");
}
prevRopeSegment = allSegments[i];
}
/********************************************************/
//interactionPoint.center = allSegments[allSegments.Length - 1].transform.position;
/********************************************************/
}
private void Initialize()
{
if (showDebug)
{
Debug.Log(base.name + " OnEnable ");
}
if (GetComponent <NetIdentity>() == null)
{
Debug.LogError("Missing NetIdentity", this);
}
Vector3[] handlePositions = GetHandlePositions();
if (handlePositions.Length < 2)
{
return;
}
Vector3 vector = handlePositions[0];
Vector3 vector2 = handlePositions[handlePositions.Length - 1];
float num = 0f;
for (int i = 0; i < handlePositions.Length - 1; i++)
{
num += (handlePositions[i] - handlePositions[i + 1]).magnitude;
}
num *= lengthMultiplier;
boneLen = num / (float)rigidSegments;
bones = new Transform[rigidSegments];
boneSleep = new NetBodySleep[rigidSegments];
boneRight = new Vector3[rigidSegments];
boneUp = new Vector3[rigidSegments];
originalPositions = new Vector3[bones.Length];
boneForward = new Vector3[rigidSegments];
Vector3 a = (vector2 - vector) / rigidSegments;
Quaternion rotation = Quaternion.LookRotation(a.normalized);
Vector3 zero = Vector3.zero;
for (int j = 0; j < rigidSegments; j++)
{
zero = vector + a * (0.5f + (float)j);
GameObject gameObject = base.gameObject;
gameObject = new GameObject("bone" + j);
gameObject.transform.SetParent(base.transform, worldPositionStays: true);
originalPositions[j] = zero;
gameObject.transform.position = zero;
gameObject.transform.rotation = rotation;
bones[j] = gameObject.transform;
gameObject.tag = "Target";
gameObject.layer = 14;
Rigidbody rigidbody = gameObject.AddComponent <Rigidbody>();
rigidbody.mass = segmentMass;
rigidbody.drag = 0.1f;
rigidbody.angularDrag = 0.1f;
CapsuleCollider capsuleCollider = gameObject.AddComponent <CapsuleCollider>();
capsuleCollider.direction = 2;
capsuleCollider.radius = radius;
capsuleCollider.height = boneLen + radius * 2f;
capsuleCollider.sharedMaterial = ropeMaterial;
if (j != 0)
{
ConfigurableJoint configurableJoint = gameObject.AddComponent <ConfigurableJoint>();
configurableJoint.connectedBody = bones[j - 1].GetComponent <Rigidbody>();
ConfigurableJointMotion configurableJointMotion2 = configurableJoint.zMotion = ConfigurableJointMotion.Locked;
configurableJointMotion2 = (configurableJoint.xMotion = (configurableJoint.yMotion = configurableJointMotion2));
configurableJointMotion2 = (configurableJoint.angularZMotion = ConfigurableJointMotion.Limited);
configurableJointMotion2 = (configurableJoint.angularXMotion = (configurableJoint.angularYMotion = configurableJointMotion2));
configurableJoint.angularXLimitSpring = new SoftJointLimitSpring
{
spring = 100f,
damper = 10f
};
configurableJoint.angularYZLimitSpring = new SoftJointLimitSpring
{
spring = 100f,
damper = 10f
};
configurableJoint.lowAngularXLimit = new SoftJointLimit
{
limit = -20f
};
configurableJoint.highAngularXLimit = new SoftJointLimit
{
limit = 20f
};
configurableJoint.angularYLimit = new SoftJointLimit
{
limit = 20f
};
configurableJoint.angularZLimit = new SoftJointLimit
{
limit = 20f
};
configurableJoint.angularXDrive = new JointDrive
{
positionSpring = 50f
};
configurableJoint.angularYZDrive = new JointDrive
{
positionSpring = 50f
};
configurableJoint.axis = new Vector3(0f, 0f, 1f);
configurableJoint.secondaryAxis = new Vector3(1f, 0f, 0f);
configurableJoint.autoConfigureConnectedAnchor = false;
configurableJoint.anchor = new Vector3(0f, 0f, (0f - boneLen) / 2f);
configurableJoint.connectedAnchor = new Vector3(0f, 0f, boneLen / 2f);
configurableJoint.projectionMode = JointProjectionMode.PositionAndRotation;
configurableJoint.projectionDistance = 0.02f;
}
boneSleep[j] = new NetBodySleep(rigidbody);
}
float num2 = (0f - boneLen) / 2f / lengthMultiplier;
int num3 = -1;
zero = Vector3.zero;
Vector3 vector3 = Vector3.zero;
for (int k = 0; k < rigidSegments; k++)
{
Vector3 vector4;
for (; num2 <= 0f; num2 += vector4.magnitude)
{
num3++;
vector4 = handlePositions[num3 + 1] - handlePositions[num3];
vector3 = vector4.normalized;
rotation = Quaternion.LookRotation(vector3);
zero = handlePositions[num3] - num2 * vector3;
}
bones[k].transform.position = zero;
bones[k].transform.rotation = rotation;
zero += vector3 * boneLen / lengthMultiplier;
num2 -= boneLen / lengthMultiplier;
}
if (fixStart && bones != null && bones.Length > 0)
{
if (showDebug)
{
Debug.Log(base.name + " Fix Start ");
}
ConfigurableJoint configurableJoint2 = bones[0].gameObject.AddComponent <ConfigurableJoint>();
ConfigurableJointMotion configurableJointMotion2 = configurableJoint2.zMotion = ConfigurableJointMotion.Locked;
configurableJointMotion2 = (configurableJoint2.xMotion = (configurableJoint2.yMotion = configurableJointMotion2));
configurableJoint2.projectionMode = JointProjectionMode.PositionAndRotation;
configurableJoint2.projectionDistance = 0.02f;
if (fixStartDir)
{
configurableJointMotion2 = (configurableJoint2.angularZMotion = ConfigurableJointMotion.Locked);
configurableJointMotion2 = (configurableJoint2.angularXMotion = (configurableJoint2.angularYMotion = configurableJointMotion2));
}
configurableJoint2.anchor = new Vector3(0f, 0f, (0f - boneLen) / 2f);
configurableJoint2.autoConfigureConnectedAnchor = false;
if (startBody != null)
{
configurableJoint2.connectedBody = startBody;
configurableJoint2.connectedAnchor = startBody.transform.InverseTransformPoint(vector);
}
else
{
configurableJoint2.connectedAnchor = vector;
}
}
if (fixEnd)
{
if (showDebug)
{
Debug.Log(base.name + " Fix End ");
}
ConfigurableJoint configurableJoint3 = bones[bones.Length - 1].gameObject.AddComponent <ConfigurableJoint>();
ConfigurableJointMotion configurableJointMotion2 = configurableJoint3.zMotion = ConfigurableJointMotion.Locked;
configurableJointMotion2 = (configurableJoint3.xMotion = (configurableJoint3.yMotion = configurableJointMotion2));
configurableJoint3.projectionMode = JointProjectionMode.PositionAndRotation;
configurableJoint3.projectionDistance = 0.02f;
if (fixEndDir)
{
configurableJointMotion2 = (configurableJoint3.angularZMotion = ConfigurableJointMotion.Locked);
configurableJointMotion2 = (configurableJoint3.angularXMotion = (configurableJoint3.angularYMotion = configurableJointMotion2));
}
configurableJoint3.anchor = new Vector3(0f, 0f, boneLen / 2f);
configurableJoint3.autoConfigureConnectedAnchor = false;
if (endBody != null)
{
configurableJoint3.connectedBody = endBody;
configurableJoint3.connectedAnchor = endBody.transform.InverseTransformPoint(vector2);
}
else
{
configurableJoint3.connectedAnchor = vector2;
}
}
if (bonePos == null || bonePos.Length != rigidSegments)
{
bonePos = new Vector3[rigidSegments];
boneRot = new Vector3[rigidSegments];
}
else
{
for (int l = 0; l < rigidSegments; l++)
{
bones[l].transform.position = base.transform.TransformPoint(bonePos[l]);
bones[l].transform.rotation = base.transform.rotation * Quaternion.Euler(boneRot[l]);
}
}
base.OnEnable();
originalState = NetStream.AllocStream();
CollectState(originalState);
}
// ConfigurableJointの値を設定する, addedは既に設定がある
void SetAttributeConfigurableJoint(PMDFormat.Joint joint, ConfigurableJoint conf, bool added)
{
if (!added)
{
SetMotionAngularLock(joint, conf);
SetDrive(joint, conf);
}
else
{
//Debug.Log("added");
}
}
void UpdateQ(bool useDeltaTime = false)
{
float dt = Time.fixedDeltaTime;
if (useDeltaTime)
{
dt = Time.deltaTime;
}
var focalTransform = _focalPoint.transform;
var focalRidgedBody = _focalPoint.GetComponent <Rigidbody>();
FocalPointPosition = focalTransform.position;
FocalPointPositionVelocity = focalRidgedBody.velocity;
var lastFocalPointRotationVelocity = FocalPointRotation;
FocalPointEulerAngles = focalTransform.eulerAngles;
FocalPointRotation = new Vector3(
((FocalPointEulerAngles.x - 180f) % 180) / 180,
((FocalPointEulerAngles.y - 180f) % 180) / 180,
((FocalPointEulerAngles.z - 180f) % 180) / 180);
FocalPointRotationVelocity = (FocalPointRotation - lastFocalPointRotationVelocity);
var topJoint = MujocoJoints[0];
//var topTransform = topJoint.Joint.transform.parent.transform;
// var topRidgedBody = topJoint.Joint.transform.parent.GetComponent<Rigidbody>();
var topTransform = topJoint.Joint.transform;
var topRidgedBody = topJoint.Joint.transform.GetComponent <Rigidbody>();
qpos[0] = topTransform.position.x;
qglobpos[0] = topTransform.position.x;
qvel[0] = topRidgedBody.velocity.x;
qpos[1] = topTransform.position.y;
qglobpos[1] = topTransform.position.y;
qvel[1] = topRidgedBody.velocity.y;
qpos[2] = ((topTransform.rotation.eulerAngles.z - 180f) % 180) / 180;
qglobpos[2] = ((topTransform.rotation.eulerAngles.z - 180f) % 180) / 180;
qvel[2] = topRidgedBody.velocity.z;
for (int i = 0; i < MujocoJoints.Count; i++)
{
// UpdateJointAngle(MujocoJoints[i], dt);
// qpos[3+i] = JointAngles[i] = MujocoJoints[i].AngleWithinRange;
// qvel[3+i] = JointVelocity[i] = MujocoJoints[i].AngularVelocityPerSecond / 1000;
var joint = MujocoJoints[i].Joint;
// var targ = joint.transform.parent.transform;
var targ = joint.transform;
float pos = 0f;
float globPos = 0f;
if (joint.axis.x != 0f)
{
pos = targ.localEulerAngles.x;
globPos = targ.eulerAngles.x;
}
else if (joint.axis.y != 0f)
{
pos = targ.localEulerAngles.y;
globPos = targ.eulerAngles.y;
}
else if (joint.axis.z != 0f)
{
pos = targ.localEulerAngles.z;
globPos = targ.eulerAngles.z;
}
HingeJoint hingeJoint = joint as HingeJoint;
ConfigurableJoint configurableJoint = joint as ConfigurableJoint;
pos = ((pos - 180f) % 180) / 180;
// pos /= 180f;
globPos = ((globPos - 180f) % 180) / 180;
if (hingeJoint != null)
{
qpos[3 + i] = pos;
qglobpos[3 + i] = globPos;
qvel[3 + i] = hingeJoint.velocity / _velocityScaler;
}
else if (configurableJoint != null)
{
var lastPos = qpos[3 + i];
qpos[3 + i] = pos;
JointAngles[i] = pos;
var lastgPos = qglobpos[3 + i];
qglobpos[3 + i] = globPos;
// var vel = joint.gameObject.GetComponent<Rigidbody>().velocity.x;
// var vel = configurableJoint.
var vel = (qpos[3 + i] - lastPos) / (dt);
qvel[3 + i] = vel;
JointVelocity[i] = vel;
}
}
for (int i = 0; i < _baseTargetPairs.Count; i++)
{
var x = _baseTargetPairs[i];
var baseRot = x.Item1.transform.rotation;
var targetRot = x.Item2.rotation;
var rotation = Quaternion.Inverse(baseRot) * targetRot;
JointRotations[i] = rotation;
var baseAngVel = x.Item1.GetComponent <Rigidbody>().angularVelocity;
var targetAngVel = x.Item2.GetComponent <Rigidbody>().angularVelocity;
var angVel = baseAngVel - targetAngVel;
angVel /= dt;
angVel /= 10000f;
JointAngularVelocities[i] = angVel;
}
}
void Start()
{
motor = GetComponent <PlayerMotor>();
joint = GetComponent <ConfigurableJoint> ();
SetJointSettings(jointSpring);
}
// 移動や回転制限
void SetMotionAngularLock(PMDFormat.Joint joint, ConfigurableJoint conf)
{
SoftJointLimit jlim;
// Motionの固定
if (joint.constrain_pos_1.x == 0f && joint.constrain_pos_2.x == 0f)
{
conf.xMotion = ConfigurableJointMotion.Locked;
}
else
{
conf.xMotion = ConfigurableJointMotion.Limited;
}
if (joint.constrain_pos_1.y == 0f && joint.constrain_pos_2.y == 0f)
{
conf.yMotion = ConfigurableJointMotion.Locked;
}
else
{
conf.yMotion = ConfigurableJointMotion.Limited;
}
if (joint.constrain_pos_1.z == 0f && joint.constrain_pos_2.z == 0f)
{
conf.zMotion = ConfigurableJointMotion.Locked;
}
else
{
conf.zMotion = ConfigurableJointMotion.Limited;
}
// 角度の固定
if (joint.constrain_rot_1.x == 0f && joint.constrain_rot_2.x == 0f)
{
conf.angularXMotion = ConfigurableJointMotion.Locked;
}
else
{
conf.angularXMotion = ConfigurableJointMotion.Limited;
float hlim = Mathf.Max(-joint.constrain_rot_1.x, -joint.constrain_rot_2.x); //回転方向が逆なので負数
float llim = Mathf.Min(-joint.constrain_rot_1.x, -joint.constrain_rot_2.x);
SoftJointLimit jhlim = new SoftJointLimit();
jhlim.limit = Mathf.Clamp(hlim * Mathf.Rad2Deg, -180.0f, 180.0f);
conf.highAngularXLimit = jhlim;
SoftJointLimit jllim = new SoftJointLimit();
jllim.limit = Mathf.Clamp(llim * Mathf.Rad2Deg, -180.0f, 180.0f);
conf.lowAngularXLimit = jllim;
}
if (joint.constrain_rot_1.y == 0f && joint.constrain_rot_2.y == 0f)
{
conf.angularYMotion = ConfigurableJointMotion.Locked;
}
else
{
// 値がマイナスだとエラーが出るので注意
conf.angularYMotion = ConfigurableJointMotion.Limited;
float lim = Mathf.Min(Mathf.Abs(joint.constrain_rot_1.y), Mathf.Abs(joint.constrain_rot_2.y)); //絶対値の小さい方
jlim = new SoftJointLimit();
jlim.limit = lim * Mathf.Clamp(Mathf.Rad2Deg, 0.0f, 180.0f);
conf.angularYLimit = jlim;
}
if (joint.constrain_rot_1.z == 0f && joint.constrain_rot_2.z == 0f)
{
conf.angularZMotion = ConfigurableJointMotion.Locked;
}
else
{
conf.angularZMotion = ConfigurableJointMotion.Limited;
float lim = Mathf.Min(Mathf.Abs(-joint.constrain_rot_1.z), Mathf.Abs(-joint.constrain_rot_2.z)); //絶対値の小さい方//回転方向が逆なので負数
jlim = new SoftJointLimit();
jlim.limit = Mathf.Clamp(lim * Mathf.Rad2Deg, 0.0f, 180.0f);
conf.angularZLimit = jlim;
}
}
private void JointRig(Rigidbody from, Rigidbody to, float distance, ref ConfigurableJoint joint)
{
JointRig(from, Vector3.zero, to, Vector3.zero, distance, ref joint);
}
/// <summary>
/// Removes the muscle with the specified joint and all muscles connected to it from PuppetMaster management. This will not destroy the body part/prop, but just release it from following the target.
/// If you call RemoveMuscleRecursive on an upper arm muscle, the entire arm will be disconnected from the rest of the body.
/// </summary>
/// <param name="joint">The joint of the muscle (and the muscles connected to it) to remove.</param>
/// <param name="attachTarget">If set to <c>true</c> , the target Transform of the first muscle will be parented to the disconnected limb.</param>
/// <param name="blockTargetAnimation">If set to <c>true</c>, will add AnimationBlocker.cs to the removed target bones. That will override animation that would otherwise still be writing on those bones.</param>
/// <param name="removeMode">Remove mode. Sever cuts the body part by disconnecting the first joint. Explode explodes the body part disconnecting all joints. Numb removes the muscles from PuppetMaster management, keeps the joints connected and disables spring and damper forces.</param>
public void RemoveMuscleRecursive(ConfigurableJoint joint, bool attachTarget, bool blockTargetAnimation = false, MuscleRemoveMode removeMode = MuscleRemoveMode.Sever)
{
if (!CheckIfInitiated())
{
return;
}
if (joint == null)
{
Debug.LogWarning("RemoveMuscleRecursive was called with a null 'joint' reference.", transform);
return;
}
if (!ContainsJoint(joint))
{
Debug.LogWarning("No Muscle with the specified joint was found, can not remove muscle.", transform);
return;
}
int index = GetMuscleIndex(joint);
Muscle[] newMuscles = new Muscle[muscles.Length - (muscles[index].childIndexes.Length + 1)];
int added = 0;
for (int i = 0; i < muscles.Length; i++)
{
if (i != index && !muscles[index].childFlags[i])
{
newMuscles[added] = muscles[i];
added++;
}
else
{
if (muscles[i].broadcaster != null)
{
muscles[i].broadcaster.enabled = false;
Destroy(muscles[i].broadcaster);
}
if (muscles[i].jointBreakBroadcaster != null)
{
muscles[i].jointBreakBroadcaster.enabled = false;
Destroy(muscles[i].jointBreakBroadcaster);
}
}
}
switch (removeMode)
{
case MuscleRemoveMode.Sever:
DisconnectJoint(muscles[index].joint);
for (int i = 0; i < muscles[index].childIndexes.Length; i++)
{
KillJoint(muscles[muscles[index].childIndexes[i]].joint);
}
break;
case MuscleRemoveMode.Explode:
DisconnectJoint(muscles[index].joint);
for (int i = 0; i < muscles[index].childIndexes.Length; i++)
{
DisconnectJoint(muscles[muscles[index].childIndexes[i]].joint);
}
break;
case MuscleRemoveMode.Numb:
KillJoint(muscles[index].joint);
for (int i = 0; i < muscles[index].childIndexes.Length; i++)
{
KillJoint(muscles[muscles[index].childIndexes[i]].joint);
}
break;
}
muscles[index].transform.parent = null;
for (int i = 0; i < muscles[index].childIndexes.Length; i++)
{
if (removeMode == MuscleRemoveMode.Explode || muscles[muscles[index].childIndexes[i]].transform.parent == transform)
{
muscles[muscles[index].childIndexes[i]].transform.parent = null;
}
}
foreach (BehaviourBase b in behaviours)
{
b.OnMuscleRemoved(muscles[index]);
for (int i = 0; i < muscles[index].childIndexes.Length; i++)
{
var c = muscles[muscles[index].childIndexes[i]];
b.OnMuscleRemoved(c);
}
}
if (attachTarget)
{
muscles[index].target.parent = muscles[index].transform;
muscles[index].target.position = muscles[index].transform.position;
muscles[index].target.rotation = muscles[index].transform.rotation * muscles[index].targetRotationRelative;
for (int i = 0; i < muscles[index].childIndexes.Length; i++)
{
var c = muscles[muscles[index].childIndexes[i]];
c.target.parent = c.transform;
c.target.position = c.transform.position;
c.target.rotation = c.transform.rotation;
}
}
if (blockTargetAnimation)
{
muscles[index].target.gameObject.AddComponent <AnimationBlocker>();
for (int i = 0; i < muscles[index].childIndexes.Length; i++)
{
var c = muscles[muscles[index].childIndexes[i]];
c.target.gameObject.AddComponent <AnimationBlocker>();
}
}
if (OnMuscleRemoved != null)
{
OnMuscleRemoved(muscles[index]);
}
for (int i = 0; i < muscles[index].childIndexes.Length; i++)
{
var c = muscles[muscles[index].childIndexes[i]];
if (OnMuscleRemoved != null)
{
OnMuscleRemoved(c);
}
}
// Enable collisions between the new muscles and the removed colliders
if (!internalCollisionsEnabled)
{
foreach (Muscle newMuscle in newMuscles)
{
foreach (Collider newMuscleCollider in newMuscle.colliders)
{
foreach (Collider removedMuscleCollider in muscles[index].colliders)
{
Physics.IgnoreCollision(newMuscleCollider, removedMuscleCollider, false);
}
for (int childMuscleIndex = 0; childMuscleIndex < muscles[index].childIndexes.Length; childMuscleIndex++)
{
foreach (Collider childMuscleCollider in muscles[childMuscleIndex].colliders)
{
Physics.IgnoreCollision(newMuscleCollider, childMuscleCollider, false);
}
}
}
}
}
muscles = newMuscles;
UpdateHierarchies();
}
void Start()
{
joint = transform.GetComponent <ConfigurableJoint>();
}
// Start is called before the first frame update
void Start()
{
_startPos = transform.localPosition;
_joint = GetComponent <ConfigurableJoint>();
}
// ばねの設定など
void SetDrive(PMDFormat.Joint joint, ConfigurableJoint conf)
{
JointDrive drive;
// Position
if (joint.spring_pos.x != 0f)
{
drive = new JointDrive();
drive.positionSpring = joint.spring_pos.x;
conf.xDrive = drive;
}
if (joint.spring_pos.y != 0f)
{
drive = new JointDrive();
drive.positionSpring = joint.spring_pos.y;
conf.yDrive = drive;
}
if (joint.spring_pos.z != 0f)
{
drive = new JointDrive();
drive.positionSpring = joint.spring_pos.z;
conf.zDrive = drive;
}
// Angular
if (joint.spring_rot.x != 0f)
{
drive = new JointDrive();
drive.mode = JointDriveMode.PositionAndVelocity;
drive.positionSpring = joint.spring_rot.x;
conf.angularXDrive = drive;
}
if (joint.spring_rot.y != 0f || joint.spring_rot.z != 0f)
{
drive = new JointDrive();
drive.mode = JointDriveMode.PositionAndVelocity;
drive.positionSpring = (joint.spring_rot.y + joint.spring_rot.z) * 0.5f;
conf.angularYZDrive = drive;
}
}
// 移動や回転制限
void SetMotionAngularLock(PMDFormat.Joint joint, ConfigurableJoint conf)
{
SoftJointLimit jlim;
// Motionの固定
if (joint.constrain_pos_1.x == 0f && joint.constrain_pos_2.x == 0f)
conf.xMotion = ConfigurableJointMotion.Locked;
else
conf.xMotion = ConfigurableJointMotion.Limited;
if (joint.constrain_pos_1.y == 0f && joint.constrain_pos_2.y == 0f)
conf.yMotion = ConfigurableJointMotion.Locked;
else
conf.yMotion = ConfigurableJointMotion.Limited;
if (joint.constrain_pos_1.z == 0f && joint.constrain_pos_2.z == 0f)
conf.zMotion = ConfigurableJointMotion.Locked;
else
conf.zMotion = ConfigurableJointMotion.Limited;
// 角度の固定
if (joint.constrain_rot_1.x == 0f && joint.constrain_rot_2.x == 0f)
conf.angularXMotion = ConfigurableJointMotion.Locked;
else
{
conf.angularXMotion = ConfigurableJointMotion.Limited;
float hlim = joint.constrain_rot_1.x > joint.constrain_rot_2.x ? joint.constrain_rot_1.x : joint.constrain_rot_2.x;
float llim = joint.constrain_rot_1.x < joint.constrain_rot_2.x ? joint.constrain_rot_1.x : joint.constrain_rot_2.x;
SoftJointLimit jhlim = new SoftJointLimit();
jhlim.limit = hlim * Mathf.Rad2Deg;
conf.highAngularXLimit = jhlim;
SoftJointLimit jllim = new SoftJointLimit();
jllim.limit = -llim * Mathf.Rad2Deg;
conf.lowAngularXLimit = jllim;
}
if (joint.constrain_rot_1.y == 0f && joint.constrain_rot_2.y == 0f)
conf.angularYMotion = ConfigurableJointMotion.Locked;
else
{
// 値がマイナスだとエラーが出るので注意
conf.angularYMotion = ConfigurableJointMotion.Limited;
float lim = joint.constrain_rot_1.y > joint.constrain_rot_2.y ? joint.constrain_rot_1.y : joint.constrain_rot_2.y;
jlim = new SoftJointLimit();
jlim.limit = lim * Mathf.Rad2Deg;
conf.angularYLimit = jlim;
}
if (joint.constrain_rot_1.z == 0f && joint.constrain_rot_2.z == 0f)
conf.angularZMotion = ConfigurableJointMotion.Locked;
else
{
conf.angularZMotion = ConfigurableJointMotion.Limited;
float lim = joint.constrain_rot_1.z > joint.constrain_rot_2.z ? joint.constrain_rot_1.z : joint.constrain_rot_2.z;
jlim = new SoftJointLimit();
jlim.limit = lim * Mathf.Rad2Deg;
conf.angularZLimit = jlim;
}
}
private void UpdatePartJoint(Part p)
{
if (!KJRJointUtils.JointAdjustmentValid(p) || p.rb == null || p.attachJoint == null)
{
return;
}
if (p.attachMethod == AttachNodeMethod.LOCKED_JOINT)
{
if (KJRJointUtils.settings.debug)
{
Debug.Log("KJR: Already processed part before: " + p.partInfo.name + " (" + p.flightID + ") -> " +
p.parent.partInfo.name + " (" + p.parent.flightID + ")");
}
return;
}
List <ConfigurableJoint> jointList;
if (p.Modules.Contains <CModuleStrut>())
{
CModuleStrut s = p.Modules.GetModule <CModuleStrut>();
if (!(s.jointTarget == null || s.jointRoot == null))
{
jointList = KJRJointUtils.GetJointListFromAttachJoint(s.strutJoint);
if (jointList != null)
{
for (int i = 0; i < jointList.Count; i++)
{
ConfigurableJoint j = jointList[i];
if (j == null)
{
continue;
}
JointDrive strutDrive = j.angularXDrive;
strutDrive.positionSpring = KJRJointUtils.settings.decouplerAndClampJointStrength;
strutDrive.maximumForce = KJRJointUtils.settings.decouplerAndClampJointStrength;
j.xDrive = j.yDrive = j.zDrive = j.angularXDrive = j.angularYZDrive = strutDrive;
j.xMotion = j.yMotion = j.zMotion = ConfigurableJointMotion.Locked;
j.angularXMotion = j.angularYMotion = j.angularZMotion = ConfigurableJointMotion.Locked;
//float scalingFactor = (s.jointTarget.mass + s.jointTarget.GetResourceMass() + s.jointRoot.mass + s.jointRoot.GetResourceMass()) * 0.01f;
j.breakForce = KJRJointUtils.settings.decouplerAndClampJointStrength;
j.breakTorque = KJRJointUtils.settings.decouplerAndClampJointStrength;
}
p.attachMethod = AttachNodeMethod.LOCKED_JOINT;
}
}
}
jointList = KJRJointUtils.GetJointListFromAttachJoint(p.attachJoint);
if (jointList == null)
{
return;
}
StringBuilder debugString = new StringBuilder();
bool addAdditionalJointToParent = KJRJointUtils.settings.multiPartAttachNodeReinforcement;
//addAdditionalJointToParent &= !(p.Modules.Contains("LaunchClamp") || (p.parent.Modules.Contains("ModuleDecouple") || p.parent.Modules.Contains("ModuleAnchoredDecoupler")));
addAdditionalJointToParent &= !p.Modules.Contains <CModuleStrut>();
float partMass = p.mass + p.GetResourceMass();
for (int i = 0; i < jointList.Count; i++)
{
ConfigurableJoint j = jointList[i];
if (j == null)
{
continue;
}
String jointType = j.GetType().Name;
Rigidbody connectedBody = j.connectedBody;
Part connectedPart = connectedBody.GetComponent <Part>() ?? p.parent;
float parentMass = connectedPart.mass + connectedPart.GetResourceMass();
if (partMass < KJRJointUtils.settings.massForAdjustment || parentMass < KJRJointUtils.settings.massForAdjustment)
{
if (KJRJointUtils.settings.debug)
{
Debug.Log("KJR: Part mass too low, skipping: " + p.partInfo.name + " (" + p.flightID + ")");
}
continue;
}
// Check attachment nodes for better orientation data
AttachNode attach = p.FindAttachNodeByPart(p.parent);
AttachNode p_attach = p.parent.FindAttachNodeByPart(p);
AttachNode node = attach ?? p_attach;
if (node == null)
{
// Check if it's a pair of coupled docking ports
var dock1 = p.Modules.GetModule <ModuleDockingNode>();
var dock2 = p.parent.Modules.GetModule <ModuleDockingNode>();
//Debug.Log(dock1 + " " + (dock1 ? ""+dock1.dockedPartUId : "?") + " " + dock2 + " " + (dock2 ? ""+dock2.dockedPartUId : "?"));
if (dock1 && dock2 && (dock1.dockedPartUId == p.parent.flightID || dock2.dockedPartUId == p.flightID))
{
attach = p.FindAttachNode(dock1.referenceAttachNode);
p_attach = p.parent.FindAttachNode(dock2.referenceAttachNode);
node = attach ?? p_attach;
}
}
// If still no node and apparently surface attached, use the normal one if it's there
if (node == null && p.attachMode == AttachModes.SRF_ATTACH)
{
node = attach = p.srfAttachNode;
}
if (KJRJointUtils.settings.debug)
{
debugString.AppendLine("Original joint from " + p.partInfo.title + " to " + p.parent.partInfo.title);
debugString.AppendLine(" " + p.partInfo.name + " (" + p.flightID + ") -> " + p.parent.partInfo.name + " (" + p.parent.flightID + ")");
debugString.AppendLine("");
debugString.AppendLine(p.partInfo.title + " Inertia Tensor: " + p.rb.inertiaTensor + " " + p.parent.partInfo.name + " Inertia Tensor: " + connectedBody.inertiaTensor);
debugString.AppendLine("");
debugString.AppendLine("Std. Joint Parameters");
debugString.AppendLine("Connected Body: " + p.attachJoint.Joint.connectedBody);
debugString.AppendLine("Attach mode: " + p.attachMode + " (was " + jointType + ")");
if (attach != null)
{
debugString.AppendLine("Attach node: " + attach.id + " - " + attach.nodeType + " " + attach.size);
}
if (p_attach != null)
{
debugString.AppendLine("Parent node: " + p_attach.id + " - " + p_attach.nodeType + " " + p_attach.size);
}
debugString.AppendLine("Anchor: " + p.attachJoint.Joint.anchor);
debugString.AppendLine("Axis: " + p.attachJoint.Joint.axis);
debugString.AppendLine("Sec Axis: " + p.attachJoint.Joint.secondaryAxis);
debugString.AppendLine("Break Force: " + p.attachJoint.Joint.breakForce);
debugString.AppendLine("Break Torque: " + p.attachJoint.Joint.breakTorque);
debugString.AppendLine("");
debugString.AppendLine("Joint Motion Locked: " + Convert.ToString(p.attachJoint.Joint.xMotion == ConfigurableJointMotion.Locked));
debugString.AppendLine("X Drive");
debugString.AppendLine("Position Spring: " + p.attachJoint.Joint.xDrive.positionSpring);
debugString.AppendLine("Position Damper: " + p.attachJoint.Joint.xDrive.positionDamper);
debugString.AppendLine("Max Force: " + p.attachJoint.Joint.xDrive.maximumForce);
debugString.AppendLine("");
debugString.AppendLine("Y Drive");
debugString.AppendLine("Position Spring: " + p.attachJoint.Joint.yDrive.positionSpring);
debugString.AppendLine("Position Damper: " + p.attachJoint.Joint.yDrive.positionDamper);
debugString.AppendLine("Max Force: " + p.attachJoint.Joint.yDrive.maximumForce);
debugString.AppendLine("");
debugString.AppendLine("Z Drive");
debugString.AppendLine("Position Spring: " + p.attachJoint.Joint.zDrive.positionSpring);
debugString.AppendLine("Position Damper: " + p.attachJoint.Joint.zDrive.positionDamper);
debugString.AppendLine("Max Force: " + p.attachJoint.Joint.zDrive.maximumForce);
debugString.AppendLine("");
debugString.AppendLine("Angular X Drive");
debugString.AppendLine("Position Spring: " + p.attachJoint.Joint.angularXDrive.positionSpring);
debugString.AppendLine("Position Damper: " + p.attachJoint.Joint.angularXDrive.positionDamper);
debugString.AppendLine("Max Force: " + p.attachJoint.Joint.angularXDrive.maximumForce);
debugString.AppendLine("");
debugString.AppendLine("Angular YZ Drive");
debugString.AppendLine("Position Spring: " + p.attachJoint.Joint.angularYZDrive.positionSpring);
debugString.AppendLine("Position Damper: " + p.attachJoint.Joint.angularYZDrive.positionDamper);
debugString.AppendLine("Max Force: " + p.attachJoint.Joint.angularYZDrive.maximumForce);
debugString.AppendLine("");
//Debug.Log(debugString.ToString());
}
float breakForce = Math.Min(p.breakingForce, connectedPart.breakingForce) * KJRJointUtils.settings.breakForceMultiplier;
float breakTorque = Math.Min(p.breakingTorque, connectedPart.breakingTorque) * KJRJointUtils.settings.breakTorqueMultiplier;
Vector3 anchor = j.anchor;
Vector3 connectedAnchor = j.connectedAnchor;
Vector3 axis = j.axis;
float radius = 0;
float area = 0;
float momentOfInertia = 0;
if (node != null)
{
// Part that owns the node. For surface attachment,
// this can only be parent if docking flips hierarchy.
Part main = (node == attach) ? p : p.parent;
// Orientation and position of the node in owner's local coords
Vector3 ndir = node.orientation.normalized;
Vector3 npos = node.position + node.offset;
// And in the current part's local coords
Vector3 dir = axis = p.transform.InverseTransformDirection(main.transform.TransformDirection(ndir));
if (node.nodeType == AttachNode.NodeType.Surface)
{
// Guessed main axis; for parts with stack nodes should be the axis of the stack
Vector3 up = KJRJointUtils.GuessUpVector(main).normalized;
// if guessed up direction is same as node direction, it's basically stack
// for instance, consider a radially-attached docking port
if (Mathf.Abs(Vector3.Dot(up, ndir)) > 0.9f)
{
radius = Mathf.Min(KJRJointUtils.CalculateRadius(main, ndir), KJRJointUtils.CalculateRadius(connectedPart, ndir));
if (radius <= 0.001)
{
radius = node.size * 1.25f;
}
area = Mathf.PI * radius * radius; //Area of cylinder
momentOfInertia = area * radius * radius / 4; //Moment of Inertia of cylinder
}
else
{
// x along surface, y along ndir normal to surface, z along surface & main axis (up)
var size1 = KJRJointUtils.CalculateExtents(main, ndir, up);
var size2 = KJRJointUtils.CalculateExtents(connectedPart, ndir, up);
// use average of the sides, since we don't know which one is used for attaching
float width1 = (size1.x + size1.z) / 2;
float width2 = (size2.x + size2.z) / 2;
if (size1.y * width1 > size2.y * width2)
{
area = size1.y * width1;
radius = Mathf.Max(size1.y, width1);
}
else
{
area = size2.y * width2;
radius = Mathf.Max(size2.y, width2);
}
momentOfInertia = area * radius / 12; //Moment of Inertia of a rectangle bending along the longer length
}
}
else
{
radius = Mathf.Min(KJRJointUtils.CalculateRadius(p, dir), KJRJointUtils.CalculateRadius(connectedPart, dir));
if (radius <= 0.001)
{
radius = node.size * 1.25f;
}
area = Mathf.PI * radius * radius; //Area of cylinder
momentOfInertia = area * radius * radius / 4; //Moment of Inertia of cylinder
}
}
//Assume part is attached along its "up" cross section; use a cylinder to approximate properties
else if (p.attachMode == AttachModes.STACK)
{
radius = Mathf.Min(KJRJointUtils.CalculateRadius(p, Vector3.up), KJRJointUtils.CalculateRadius(connectedPart, Vector3.up));
if (radius <= 0.001)
{
radius = node.size * 1.25f;
}
area = Mathf.PI * radius * radius; //Area of cylinder
momentOfInertia = area * radius * radius / 4; //Moment of Inertia of cylinder
}
else if (p.attachMode == AttachModes.SRF_ATTACH)
{
// x,z sides, y along main axis
Vector3 up1 = KJRJointUtils.GuessUpVector(p);
var size1 = KJRJointUtils.CalculateExtents(p, up1);
Vector3 up2 = KJRJointUtils.GuessUpVector(connectedPart);
var size2 = KJRJointUtils.CalculateExtents(connectedPart, up2);
// use average of the sides, since we don't know which one is used for attaching
float width1 = (size1.x + size1.z) / 2;
float width2 = (size2.x + size2.z) / 2;
if (size1.y * width1 > size2.y * width2)
{
area = size1.y * width1;
radius = Mathf.Max(size1.y, width1);
}
else
{
area = size2.y * width2;
radius = Mathf.Max(size2.y, width2);
}
momentOfInertia = area * radius / 12; //Moment of Inertia of a rectangle bending along the longer length
}
if (KJRJointUtils.settings.useVolumeNotArea) //If using volume, raise al stiffness-affecting parameters to the 1.5 power
{
area = Mathf.Pow(area, 1.5f);
momentOfInertia = Mathf.Pow(momentOfInertia, 1.5f);
}
breakForce = Mathf.Max(KJRJointUtils.settings.breakStrengthPerArea * area, breakForce);
breakTorque = Mathf.Max(KJRJointUtils.settings.breakTorquePerMOI * momentOfInertia, breakTorque);
JointDrive angDrive = j.angularXDrive;
angDrive.positionSpring = Mathf.Max(momentOfInertia * KJRJointUtils.settings.angularDriveSpring, angDrive.positionSpring);
angDrive.positionDamper = Mathf.Max(momentOfInertia * KJRJointUtils.settings.angularDriveDamper * 0.1f, angDrive.positionDamper);
angDrive.maximumForce = breakTorque;
/*float moi_avg = p.rb.inertiaTensor.magnitude;
*
* moi_avg += (p.transform.localToWorldMatrix.MultiplyPoint(p.CoMOffset) - p.parent.transform.position).sqrMagnitude * p.rb.mass;
*
* if (moi_avg * 2f / drive.positionDamper < 0.08f)
* {
* drive.positionDamper = moi_avg / (0.04f);
*
* drive.positionSpring = drive.positionDamper * drive.positionDamper / moi_avg;
* }*/
j.angularXDrive = j.angularYZDrive = j.slerpDrive = angDrive;
JointDrive linDrive = j.xDrive;
linDrive.maximumForce = breakForce;
j.xDrive = j.yDrive = j.zDrive = linDrive;
SoftJointLimit lim = new SoftJointLimit();
lim.limit = 0;
lim.bounciness = 0;
SoftJointLimitSpring limSpring = new SoftJointLimitSpring();
limSpring.spring = 0;
limSpring.damper = 0;
j.linearLimit = j.angularYLimit = j.angularZLimit = j.lowAngularXLimit = j.highAngularXLimit = lim;
j.linearLimitSpring = j.angularYZLimitSpring = j.angularXLimitSpring = limSpring;
j.targetAngularVelocity = Vector3.zero;
j.targetVelocity = Vector3.zero;
j.targetRotation = Quaternion.identity;
j.targetPosition = Vector3.zero;
j.breakForce = breakForce;
j.breakTorque = breakTorque;
p.attachJoint.SetBreakingForces(j.breakForce, j.breakTorque);
p.attachMethod = AttachNodeMethod.LOCKED_JOINT;
if (addAdditionalJointToParent && p.parent.parent != null)
{
addAdditionalJointToParent = false;
if (!KJRJointUtils.JointAdjustmentValid(p.parent) || !KJRJointUtils.JointAdjustmentValid(p.parent.parent))
{
continue;
}
/*if (ValidDecoupler(p) || ValidDecoupler(p.parent))
* continue;*/
Part newConnectedPart = p.parent.parent;
bool massRatioBelowThreshold = false;
int numPartsFurther = 0;
float partMaxMass = KJRJointUtils.MaximumPossiblePartMass(p);
List <Part> partsCrossed = new List <Part>();
List <Part> possiblePartsCrossed = new List <Part>();
partsCrossed.Add(p);
partsCrossed.Add(p.parent);
partsCrossed.Add(newConnectedPart);
Rigidbody connectedRb = newConnectedPart.rb;
do
{
float massRat1, massRat2;
massRat1 = partMaxMass / newConnectedPart.mass;
if (massRat1 < 1)
{
massRat1 = 1 / massRat1;
}
massRat2 = p.mass / KJRJointUtils.MaximumPossiblePartMass(newConnectedPart);
if (massRat2 < 1)
{
massRat2 = 1 / massRat2;
}
if (massRat1 > KJRJointUtils.settings.stiffeningExtensionMassRatioThreshold || massRat2 > KJRJointUtils.settings.stiffeningExtensionMassRatioThreshold)
{
if (newConnectedPart.parent != null)
{
if (!KJRJointUtils.JointAdjustmentValid(newConnectedPart.parent))
{
break;
}
newConnectedPart = newConnectedPart.parent;
if (newConnectedPart.rb == null)
{
possiblePartsCrossed.Add(newConnectedPart);
}
else
{
connectedRb = newConnectedPart.rb;
partsCrossed.AddRange(possiblePartsCrossed);
partsCrossed.Add(newConnectedPart);
possiblePartsCrossed.Clear();
}
}
else
{
break;
}
numPartsFurther++;
}
else
{
massRatioBelowThreshold = true;
}
} while (!massRatioBelowThreshold);// && numPartsFurther < 5);
if (connectedRb != null && !multiJointManager.CheckMultiJointBetweenParts(p, newConnectedPart))
{
ConfigurableJoint newJoint = p.gameObject.AddComponent <ConfigurableJoint>();
newJoint.connectedBody = connectedRb;
newJoint.axis = Vector3.right;
newJoint.secondaryAxis = Vector3.forward;
newJoint.anchor = Vector3.zero;
newJoint.connectedAnchor = p.transform.worldToLocalMatrix.MultiplyPoint(newConnectedPart.transform.position);
//if(massRatioBelowThreshold)
//{
newJoint.angularXDrive = newJoint.angularYZDrive = newJoint.slerpDrive = j.angularXDrive;
newJoint.xDrive = j.xDrive;
newJoint.yDrive = j.yDrive;
newJoint.zDrive = j.zDrive;
newJoint.linearLimit = newJoint.angularYLimit = newJoint.angularZLimit = newJoint.lowAngularXLimit = newJoint.highAngularXLimit = lim;
/*newJoint.targetAngularVelocity = Vector3.zero;
* newJoint.targetVelocity = Vector3.zero;
* newJoint.targetRotation = Quaternion.identity;
* newJoint.targetPosition = Vector3.zero;*/
/*}
* else
* {
* newJoint.xMotion = newJoint.yMotion = newJoint.zMotion = ConfigurableJointMotion.Locked;
* newJoint.angularXMotion = newJoint.angularYMotion = newJoint.angularZMotion = ConfigurableJointMotion.Locked;
* }*/
newJoint.breakForce = breakForce;
newJoint.breakTorque = breakTorque;
//jointList.Add(newJoint);
for (int k = 0; k < partsCrossed.Count; k++)
{
multiJointManager.RegisterMultiJoint(partsCrossed[k], newJoint);
}
}
/*if(p.symmetryCounterparts != null && p.symmetryCounterparts.Count > 0)
* {
* Part linkPart = null;
* Vector3 center = p.transform.position;
* float cross = float.NegativeInfinity;
* for(int k = 0; k < p.symmetryCounterparts.Count; k++)
* {
* center += p.symmetryCounterparts[k].transform.position;
* }
* center /= (p.symmetryCounterparts.Count + 1);
*
* for(int k = 0; k < p.symmetryCounterparts.Count; k++)
* {
* Part counterPart = p.symmetryCounterparts[k];
* if (counterPart.parent == p.parent && counterPart.rb != null)
* {
* float tmpCross = Vector3.Dot(Vector3.Cross(center - p.transform.position, counterPart.transform.position - p.transform.position), p.transform.up);
* if(tmpCross > cross)
* {
* cross = tmpCross;
* linkPart = counterPart;
* }
* }
* }
* if (linkPart)
* {
* Rigidbody rigidBody = linkPart.rb;
* if (!linkPart.rb)
* continue;
* ConfigurableJoint newJoint;
*
* newJoint = p.gameObject.AddComponent<ConfigurableJoint>();
*
* newJoint.connectedBody = rigidBody;
* newJoint.anchor = Vector3.zero;
* newJoint.axis = Vector3.right;
* newJoint.secondaryAxis = Vector3.forward;
* newJoint.breakForce = KJRJointUtils.decouplerAndClampJointStrength;
* newJoint.breakTorque = KJRJointUtils.decouplerAndClampJointStrength;
*
* newJoint.xMotion = newJoint.yMotion = newJoint.zMotion = ConfigurableJointMotion.Locked;
* newJoint.angularXMotion = newJoint.angularYMotion = newJoint.angularZMotion = ConfigurableJointMotion.Locked;
*
* multiJointManager.RegisterMultiJoint(p, newJoint);
* multiJointManager.RegisterMultiJoint(linkPart, newJoint);
* }
* }*/
}
if (KJRJointUtils.settings.debug)
{
debugString.AppendLine("Updated joint from " + p.partInfo.title + " to " + p.parent.partInfo.title);
debugString.AppendLine(" " + p.partInfo.name + " (" + p.flightID + ") -> " + p.parent.partInfo.name + " (" + p.parent.flightID + ")");
debugString.AppendLine("");
debugString.AppendLine(p.partInfo.title + " Inertia Tensor: " + p.rb.inertiaTensor + " " + p.parent.partInfo.name + " Inertia Tensor: " + connectedBody.inertiaTensor);
debugString.AppendLine("");
debugString.AppendLine("Std. Joint Parameters");
debugString.AppendLine("Connected Body: " + p.attachJoint.Joint.connectedBody);
debugString.AppendLine("Attach mode: " + p.attachMode + " (was " + jointType + ")");
if (attach != null)
{
debugString.AppendLine("Attach node: " + attach.id + " - " + attach.nodeType + " " + attach.size);
}
if (p_attach != null)
{
debugString.AppendLine("Parent node: " + p_attach.id + " - " + p_attach.nodeType + " " + p_attach.size);
}
debugString.AppendLine("Anchor: " + p.attachJoint.Joint.anchor);
debugString.AppendLine("Axis: " + p.attachJoint.Joint.axis);
debugString.AppendLine("Sec Axis: " + p.attachJoint.Joint.secondaryAxis);
debugString.AppendLine("Break Force: " + p.attachJoint.Joint.breakForce);
debugString.AppendLine("Break Torque: " + p.attachJoint.Joint.breakTorque);
debugString.AppendLine("");
debugString.AppendLine("Joint Motion Locked: " + Convert.ToString(p.attachJoint.Joint.xMotion == ConfigurableJointMotion.Locked));
debugString.AppendLine("Angular Drive");
debugString.AppendLine("Position Spring: " + angDrive.positionSpring);
debugString.AppendLine("Position Damper: " + angDrive.positionDamper);
debugString.AppendLine("Max Force: " + angDrive.maximumForce);
debugString.AppendLine("");
debugString.AppendLine("Cross Section Properties");
debugString.AppendLine("Radius: " + radius);
debugString.AppendLine("Area: " + area);
debugString.AppendLine("Moment of Inertia: " + momentOfInertia);
}
}
if (KJRJointUtils.settings.debug)
{
Debug.Log(debugString.ToString());
}
}
protected override bool DetectSetup()
{
finalDirection = (direction == ButtonDirection.autodetect) ? DetectDirection() : direction;
if (finalDirection == ButtonDirection.autodetect)
{
activationDir = Vector3.zero;
return(false);
}
if (direction != ButtonDirection.autodetect)
{
activationDir = CalculateActivationDir();
}
if (cf)
{
cf.force = GetForceVector();
}
if (Application.isPlaying)
{
cj = GetComponent <ConfigurableJoint>();
bool recreate = false;
Rigidbody oldBody = null;
Vector3 oldAnchor = Vector3.zero;
Vector3 oldAxis = Vector3.zero;
if (cj)
{
// save old values, needs to be recreated
oldBody = cj.connectedBody;
oldAnchor = cj.anchor;
oldAxis = cj.axis;
DestroyImmediate(cj);
recreate = true;
}
// since limit applies to both directions object needs to be moved halfway to activation before adding joint
transform.position = transform.position + activationDir.normalized * activationDistance * 0.5f;
cj = gameObject.AddComponent <ConfigurableJoint>();
if (recreate)
{
cj.connectedBody = oldBody;
cj.anchor = oldAnchor;
cj.axis = oldAxis;
}
if (connectedTo)
{
cj.connectedBody = connectedTo.GetComponent <Rigidbody>();
}
SoftJointLimit sjl = new SoftJointLimit();
sjl.limit = activationDistance * 0.501f; // set limit to half (since it applies to both directions) and a tiny bit larger since otherwise activation distance might be missed
cj.linearLimit = sjl;
cj.angularXMotion = ConfigurableJointMotion.Locked;
cj.angularYMotion = ConfigurableJointMotion.Locked;
cj.angularZMotion = ConfigurableJointMotion.Locked;
cj.xMotion = ConfigurableJointMotion.Locked;
cj.yMotion = ConfigurableJointMotion.Locked;
cj.zMotion = ConfigurableJointMotion.Locked;
switch (finalDirection)
{
case ButtonDirection.x:
case ButtonDirection.negX:
if (Mathf.RoundToInt(Mathf.Abs(transform.right.x)) == 1)
{
cj.xMotion = ConfigurableJointMotion.Limited;
}
else if (Mathf.RoundToInt(Mathf.Abs(transform.up.x)) == 1)
{
cj.yMotion = ConfigurableJointMotion.Limited;
}
else if (Mathf.RoundToInt(Mathf.Abs(transform.forward.x)) == 1)
{
cj.zMotion = ConfigurableJointMotion.Limited;
}
break;
case ButtonDirection.y:
case ButtonDirection.negY:
if (Mathf.RoundToInt(Mathf.Abs(transform.right.y)) == 1)
{
cj.xMotion = ConfigurableJointMotion.Limited;
}
else if (Mathf.RoundToInt(Mathf.Abs(transform.up.y)) == 1)
{
cj.yMotion = ConfigurableJointMotion.Limited;
}
else if (Mathf.RoundToInt(Mathf.Abs(transform.forward.y)) == 1)
{
cj.zMotion = ConfigurableJointMotion.Limited;
}
break;
case ButtonDirection.z:
case ButtonDirection.negZ:
if (Mathf.RoundToInt(Mathf.Abs(transform.right.z)) == 1)
{
cj.xMotion = ConfigurableJointMotion.Limited;
}
else if (Mathf.RoundToInt(Mathf.Abs(transform.up.z)) == 1)
{
cj.yMotion = ConfigurableJointMotion.Limited;
}
else if (Mathf.RoundToInt(Mathf.Abs(transform.forward.z)) == 1)
{
cj.zMotion = ConfigurableJointMotion.Limited;
}
break;
}
jointDelta = transform.localPosition - cj.connectedAnchor;
}
return(true);
}
private void Start()
{
joint = GetComponent <ConfigurableJoint>();
movement = GetComponent <PlayerMovement>();
SetJointSettings(jointSpring);
}
/// <summary>
/// Adds a PropMuscle to the puppet at runtime. If Vector3.zero passed for additionalPinOffset, additional pin will not be added.
/// </summary>
public bool AddPropMuscle(ConfigurableJoint addPropMuscleTo, Vector3 position, Quaternion rotation, Vector3 additionalPinOffset, Transform targetParent = null, PuppetMasterProp initiateWithProp = null)
{
if (!initiated)
{
Debug.LogError("Can not add Prop Muscle to PuppetMaster that has not been initiated! Please use Start() instead of Awake() or PuppetMaster.OnPostInitiate delegate to call AddPropMuscle.", transform);
return(false);
}
if (addPropMuscleTo != null)
{
bool isFlat = HierarchyIsFlat();
var addToMuscle = GetMuscle(addPropMuscleTo);
if (addToMuscle != null)
{
GameObject go = new GameObject("Prop Muscle " + addPropMuscleTo.name);
go.layer = addPropMuscleTo.gameObject.layer;
go.transform.parent = isFlat ? transform : addPropMuscleTo.transform;
go.transform.position = position;
go.transform.rotation = rotation;
go.AddComponent <Rigidbody>();
GameObject target = new GameObject("Prop Muscle Target " + addPropMuscleTo.name);
target.gameObject.layer = addToMuscle.target.gameObject.layer;
target.transform.parent = targetParent != null? targetParent: addToMuscle.target;
target.transform.position = go.transform.position;
target.transform.rotation = go.transform.rotation;
ConfigurableJoint joint = go.AddComponent <ConfigurableJoint>();
joint.xMotion = ConfigurableJointMotion.Locked;
joint.yMotion = ConfigurableJointMotion.Locked;
joint.zMotion = ConfigurableJointMotion.Locked;
joint.angularXMotion = ConfigurableJointMotion.Locked;
joint.angularYMotion = ConfigurableJointMotion.Locked;
joint.angularZMotion = ConfigurableJointMotion.Locked;
Muscle.Props props = new Muscle.Props();
props.group = Muscle.Group.Prop;
AddMuscle(joint, target.transform, addPropMuscleTo.GetComponent <Rigidbody>(), targetParent != null ? targetParent : addToMuscle.target, props, false, true);
muscles[muscles.Length - 1].isPropMuscle = true;
var propMuscle = go.AddComponent <PropMuscle>();
propMuscle.puppetMaster = this;
propMuscle.additionalPinOffset = additionalPinOffset;
propMuscle.currentProp = initiateWithProp;
if (additionalPinOffset != Vector3.zero)
{
propMuscle.AddAdditionalPin();
}
Array.Resize(ref propMuscles, propMuscles.Length + 1);
propMuscles[propMuscles.Length - 1] = propMuscle;
propMuscle.OnInitiate();
return(true);
}
else
{
Debug.LogError("Can't add Prop Muscle to a ConfigurableJoint that is not in the list of PuppetMaster.muscles.", transform);
return(false);
}
}
else
{
Debug.LogError("Please assign the ConfigurableJoint of the muscle you wish to add the Prop Muscle to.", transform);
return(false);
}
}
/// <summary>
/// NB! Make sure to call this from FixedUpdate!
/// Creates a new muscle for the specified "joint" and targets it to the "target". The joint will be connected to the specified "connectTo" Muscle.
/// Note that the joint will be binded to it's current position and rotation relative to the "connectTo", so make sure the added object is positioned correctly when calling this.
/// This method allocates memory, avoid using it each frame.
/// </summary>
public void AddMuscle(ConfigurableJoint joint, Transform target, Rigidbody connectTo, Transform targetParent, Muscle.Props muscleProps = null, bool forceTreeHierarchy = false, bool forceLayers = true)
{
if (!CheckIfInitiated())
{
return;
}
if (!initiated)
{
Debug.LogWarning("PuppetMaster has not been initiated.", transform);
return;
}
if (ContainsJoint(joint))
{
Debug.LogWarning("Joint " + joint.name + " is already used by a Muscle", transform);
return;
}
if (target == null)
{
Debug.LogWarning("AddMuscle was called with a null 'target' reference.", transform);
return;
}
if (connectTo == joint.GetComponent <Rigidbody>())
{
Debug.LogWarning("ConnectTo is the joint's own Rigidbody, can not add muscle.", transform);
return;
}
if (!isActive)
{
Debug.LogWarning("Adding muscles to inactive PuppetMasters is not currently supported.", transform);
return;
}
if (muscleProps == null)
{
muscleProps = new Muscle.Props();
}
Muscle muscle = new Muscle();
muscle.props = muscleProps;
muscle.joint = joint;
muscle.target = target;
muscle.joint.transform.parent = (hierarchyIsFlat || connectTo == null) && !forceTreeHierarchy? transform: connectTo.transform;
if (forceLayers)
{
joint.gameObject.layer = gameObject.layer; //@todo what if collider is on a child gameobject?
target.gameObject.layer = targetRoot.gameObject.layer;
}
if (connectTo != null)
{
muscle.target.parent = targetParent;
Vector3 relativePosition = GetMuscle(connectTo).transform.InverseTransformPoint(muscle.target.position);
Quaternion relativeRotation = Quaternion.Inverse(GetMuscle(connectTo).transform.rotation) * muscle.target.rotation;
joint.transform.position = connectTo.transform.TransformPoint(relativePosition);
joint.transform.rotation = connectTo.transform.rotation * relativeRotation;
joint.connectedBody = connectTo;
}
muscle.Initiate(muscles);
if (connectTo != null)
{
muscle.rigidbody.velocity = connectTo.velocity;
muscle.rigidbody.angularVelocity = connectTo.angularVelocity;
}
// Ignore internal collisions
if (!internalCollisions)
{
for (int i = 0; i < muscles.Length; i++)
{
muscle.IgnoreCollisions(muscles[i], true);
}
}
Array.Resize(ref muscles, muscles.Length + 1);
muscles[muscles.Length - 1] = muscle;
// Update angular limit ignoring
muscle.IgnoreAngularLimits(!angularLimits);
if (behaviours.Length > 0)
{
muscle.broadcaster = muscle.joint.gameObject.AddComponent <MuscleCollisionBroadcaster>();
muscle.broadcaster.puppetMaster = this;
muscle.broadcaster.muscleIndex = muscles.Length - 1;
}
muscle.jointBreakBroadcaster = muscle.joint.gameObject.AddComponent <JointBreakBroadcaster>();
muscle.jointBreakBroadcaster.puppetMaster = this;
muscle.jointBreakBroadcaster.muscleIndex = muscles.Length - 1;
UpdateHierarchies();
CheckMassVariation(100f, true);
foreach (BehaviourBase b in behaviours)
{
b.OnMuscleAdded(muscle);
}
}
private void FixedUpdate()
{
if (targetAngle == 180f)
{
targetAngle = -180f;
}
if (Quaternion.Euler(targetAngle * axis) != movingMeshTransform.localRotation &&
Quaternion.Euler(targetAngle * axis) != targetRotation)
{
Move();
}
if (!isMoving && !isFinished)
{
/*foreach (Part p in this.part.children)
* {
* p.transform.SetParent(null, true);
* p.attachJoint.Joint.connectedBody = this.part.rb;
* p.rb.isKinematic = false;
* }*/
Stack <Part> stack = new Stack <Part>();
stack.Push(this.part);
while (stack.Count > 0)
{
Part item = stack.Pop();
if (item == this.part)
{
Part child = moveNode.attachedPart;
ConfigurableJoint partJoint = child.attachJoint.Joint;
child.transform.SetParent(null, true);
partJoint.connectedBody = part.rb;
partJoint.autoConfigureConnectedAnchor = true; // auto set anchor
child.rb.isKinematic = false;
stack.Push(child);
}
else
{
foreach (Part child in item.children)
{
if (child.PhysicsSignificance == 1) // skip physics less parts
{
continue;
}
ConfigurableJoint partJoint = child.attachJoint.Joint;
child.transform.SetParent(null, true);
partJoint.connectedBody = item.rb;
//partJoint.autoConfigureConnectedAnchor = true;
child.rb.isKinematic = false;
stack.Push(child);
}
}
}
//isMoving = false;
isFinished = true;
}
else if (isMoving)
{
if (movingMeshTransform.localRotation == targetRotation)
{
isMoving = false;
}
else
{
movingMeshTransform.localRotation = Quaternion.RotateTowards(movingMeshTransform.localRotation, targetRotation, speed * TimeWarp.deltaTime);
}
}
}
// ConfigurableJointの値を設定する
void SetAttributeConfigurableJoint(PMDFormat.Joint joint, ConfigurableJoint conf)
{
SetMotionAngularLock(joint, conf);
SetDrive(joint, conf);
}
private static void SetAnchor(ConfigurableJoint joint, Rigidbody desiredObject, Vector3 anchor)
{
joint.autoConfigureConnectedAnchor = false;
joint.connectedAnchor = desiredObject.transform.InverseTransformVector(anchor);
}
private void JointRig(Rigidbody from, Vector3 fromAnchor, Rigidbody to, Vector3 toAnchor, float distance, ref ConfigurableJoint joint)
{
if (joint == null)
{
joint = from.gameObject.AddComponent <ConfigurableJoint>();
}
else
{
Debug.Assert(joint.gameObject == from.gameObject);
}
joint.autoConfigureConnectedAnchor = false;
joint.connectedBody = to;
joint.anchor = fromAnchor;
joint.connectedAnchor = toAnchor;
joint.xMotion = ConfigurableJointMotion.Limited;
joint.yMotion = ConfigurableJointMotion.Limited;
joint.zMotion = ConfigurableJointMotion.Limited;
joint.linearLimit = new SoftJointLimit()
{
limit = distance,
bounciness = 0,
contactDistance = 0
};
joint.linearLimitSpring = new SoftJointLimitSpring()
{
spring = 0,
damper = 0
};
joint.projectionMode = JointProjectionMode.PositionAndRotation;
joint.projectionDistance = 0;
joint.projectionAngle = 0;
joint.enablePreprocessing = false;
}
void UpdateQ()
{
if (MujocoJoints == null || MujocoJoints.Count == 0)
{
return;
}
float dt = Time.fixedDeltaTime;
FocalPointMaxDistanceTraveled = Mathf.Max(FocalPointMaxDistanceTraveled, FocalPoint.transform.position.x);
var topJoint = MujocoJoints[0];
//var topTransform = topJoint.Joint.transform.parent.transform;
// var topRidgedBody = topJoint.Joint.transform.parent.GetComponent<Rigidbody>();
var topTransform = topJoint.Joint.transform;
var topRidgedBody = topJoint.Joint.transform.GetComponent <Rigidbody>();
qpos[0] = topTransform.position.x;
qglobpos[0] = topTransform.position.x;
qvel[0] = topRidgedBody.velocity.x;
qpos[1] = topTransform.position.y;
qglobpos[1] = topTransform.position.y;
qvel[1] = topRidgedBody.velocity.y;
qpos[2] = ((topTransform.rotation.eulerAngles.z - 180f) % 180) / 180;
qglobpos[2] = ((topTransform.rotation.eulerAngles.z - 180f) % 180) / 180;
qvel[2] = topRidgedBody.velocity.z;
for (int i = 0; i < MujocoJoints.Count; i++)
{
// UpdateJointAngle(MujocoJoints[i], dt);
// qpos[3+i] = JointAngles[i] = MujocoJoints[i].AngleWithinRange;
// qvel[3+i] = JointVelocity[i] = MujocoJoints[i].AngularVelocityPerSecond / 1000;
var joint = MujocoJoints[i].Joint;
// var targ = joint.transform.parent.transform;
var targ = joint.transform;
float pos = 0f;
float globPos = 0f;
if (joint.axis.x != 0f)
{
pos = targ.localEulerAngles.x;
globPos = targ.eulerAngles.x;
}
else if (joint.axis.y != 0f)
{
pos = targ.localEulerAngles.y;
globPos = targ.eulerAngles.y;
}
else if (joint.axis.z != 0f)
{
pos = targ.localEulerAngles.z;
globPos = targ.eulerAngles.z;
}
ConfigurableJoint configurableJoint = joint as ConfigurableJoint;
pos = ((pos - 180f) % 180) / 180;
// pos /= 180f;
globPos = ((globPos - 180f) % 180) / 180;
var lastPos = qpos[3 + i];
qpos[3 + i] = pos;
JointAngles[i] = pos;
var lastgPos = qglobpos[3 + i];
qglobpos[3 + i] = globPos;
// var vel = joint.gameObject.GetComponent<Rigidbody>().velocity.x;
// var vel = configurableJoint.
var vel = (qpos[3 + i] - lastPos) / (dt);
qvel[3 + i] = vel;
JointVelocity[i] = vel;
}
for (int i = 0; i < _baseTargetPairs.Count; i++)
{
var x = _baseTargetPairs[i];
var baseRot = x.Item1.transform.rotation;
var targetRot = x.Item2.rotation;
var rotation = Quaternion.Inverse(baseRot) * targetRot;
JointRotations[i] = rotation;
var baseAngVel = x.Item1.GetComponent <Rigidbody>().angularVelocity;
var targetAngVel = x.Item2.GetComponent <Rigidbody>().angularVelocity;
var angVel = baseAngVel - targetAngVel;
angVel /= dt;
angVel /= 10000f;
JointAngularVelocities[i] = angVel;
}
}
static public void ApplyAction(MujocoJoint mJoint, float?target = null)
{
HingeJoint hingeJoint = mJoint.Joint as HingeJoint;
ConfigurableJoint configurableJoint = mJoint.Joint as ConfigurableJoint;
if (configurableJoint != null)
{
if (!target.HasValue) // handle random
{
target = Random.value * 2 - 1;
}
target = Mathf.Clamp(target.Value, -3f, 3f);
var t = configurableJoint.targetAngularVelocity;
t.x = target.Value * _velocityScaler;
configurableJoint.targetAngularVelocity = t;
var angX = configurableJoint.angularXDrive;
angX.positionSpring = 1f;
var scale = mJoint.MaximumForce * Mathf.Pow(Mathf.Abs(target.Value), 3);
angX.positionDamper = Mathf.Max(1f, scale);
angX.maximumForce = Mathf.Max(1f, scale);
configurableJoint.angularXDrive = angX;
return;
}
else if (hingeJoint == null)
{
return;
}
if (hingeJoint.useSpring)
{
var ctrlRangeMin = -1f;
var ctrlRangeMax = 1f;
// var ctrlRangeMin = 0f;
// var ctrlRangeMax = 1f;
var inputScale = ctrlRangeMax - ctrlRangeMin;
if (!target.HasValue) // handle random
{
target = ctrlRangeMin + (Random.value * inputScale);
}
var inputTarget = Mathf.Clamp(target.Value, ctrlRangeMin, ctrlRangeMax);
if (ctrlRangeMin < 0)
{
inputTarget = Mathf.Abs(ctrlRangeMin) + inputTarget;
}
else
{
inputTarget = inputTarget - Mathf.Abs(ctrlRangeMin);
}
inputTarget /= inputScale;
JointSpring js;
js = hingeJoint.spring;
var min = hingeJoint.limits.min;
var max = hingeJoint.limits.max;
var outputScale = max - min;
var outputTarget = min + (inputTarget * outputScale);
js.targetPosition = outputTarget;
hingeJoint.spring = js;
}
else if (hingeJoint.useMotor)
{
if (!target.HasValue) // handle random
{
target = Random.value * 2 - 1;
}
target = Mathf.Clamp(target.Value, -1f, 1f);
// target = Mathf.Clamp(target.Value, 0f, 1f);
// target *= 2;
// target -= 1f;
JointMotor jm;
jm = hingeJoint.motor;
jm.targetVelocity = target.Value * _velocityScaler;
hingeJoint.motor = jm;
}
}
private void SetupJoint()
{
_joint = gameObject.AddComponent <ConfigurableJoint>();
_joint.connectedBody = ConnectedBody;
//_joint.anchor = StartPosition;
_joint.autoConfigureConnectedAnchor = false;
var worldStartPosition = StartPosition;
if (transform.parent)
{
worldStartPosition = transform.parent.TransformPoint(StartPosition);
}
var worldEndPosition = EndPosition;
if (transform.parent)
{
worldEndPosition = transform.parent.TransformPoint(EndPosition);
}
if (ConnectedBody)
{
_joint.connectedAnchor = ConnectedBody.transform.InverseTransformPoint(worldStartPosition);
}
else
{
_joint.connectedAnchor = worldStartPosition;
}
_joint.SetXDrive(Spring, Damper, Spring);
_joint.LimitXMotion();
_joint.LockYMotion();
_joint.LockZMotion();
_joint.LockAllAngularMotion();
_joint.axis = _axis;
_joint.secondaryAxis = _joint.axis.OrthogonalVector();
_joint.SetLinearLimit(Vector3.Distance(StartPosition, EndPosition));
_limitJoint = gameObject.AddComponent <ConfigurableJoint>();
_limitJoint.connectedBody = ConnectedBody;
//_limitJoint.anchor = EndPosition;
_limitJoint.autoConfigureConnectedAnchor = false;
if (ConnectedBody)
{
_limitJoint.connectedAnchor = ConnectedBody.transform.InverseTransformPoint(worldEndPosition);
}
else
{
_limitJoint.connectedAnchor = worldEndPosition;
}
_limitJoint.LockYMotion();
_limitJoint.LockZMotion();
_limitJoint.LockAllAngularMotion();
_limitJoint.axis = _axis;
_limitJoint.secondaryAxis = _joint.axis.OrthogonalVector();
_limitJoint.LimitXMotion();
_limitJoint.SetLinearLimit(Vector3.Distance(StartPosition, EndPosition));
}
public void Initiate(BehaviourBase behaviour, Settings settings, Rigidbody Ibody, Rigidbody[] rigidbodies, ConfigurableJoint joint, Transform[] copPoints, PressureSensor pressureSensor)
{
this.behaviour = behaviour;
this.settings = settings;
this.Ibody = Ibody;
this.rigidbodies = rigidbodies;
this.joint = joint;
this.copPoints = copPoints;
this.pressureSensor = pressureSensor;
toJointSpace = PhysXTools.ToJointSpace(joint);
behaviour.OnPreFixedUpdate += Solve;
}
public static void SetTargetRotationEuler(this ConfigurableJoint joint, float x, float y, float z)
{
joint.targetRotation = Quaternion.Euler(x, y, z);
}
private void Awake()
{
configurableJoint = GetComponent <ConfigurableJoint>();
referencePosition = Vector3.Dot(transform.localPosition, configurableJoint.axis);
}
public static void SetTargetPosition(this ConfigurableJoint joint, float x, float y, float z)
{
joint.targetPosition = new Vector3(x, y, z);
}
