private void ReleaseJoint()
{
muscle.connectedBody = null;
muscle.targetRotation = Quaternion.identity;
JointDrive j = new JointDrive();
j.positionSpring = 0f;
#if UNITY_5_2
j.mode = JointDriveMode.None;
#endif
muscle.slerpDrive = j;
muscle.xMotion = ConfigurableJointMotion.Free;
muscle.yMotion = ConfigurableJointMotion.Free;
muscle.zMotion = ConfigurableJointMotion.Free;
muscle.angularXMotion = ConfigurableJointMotion.Free;
muscle.angularYMotion = ConfigurableJointMotion.Free;
muscle.angularZMotion = ConfigurableJointMotion.Free;
}
void onDragEnd(Rigidbody body, Vector3 worldPos, Vector3 worldNormal)
{
if (!draggedBody || body == draggedBody || draggedBody.gameObject == null)
{
return;
}
//Attach sound
Quaternion backupRotation = draggedBody.transform.localRotation;
Vector3 n1 = normalOnDraggedBody;
Vector3 n2 = draggedBody.transform.InverseTransformVector(worldNormal);
draggedBody.transform.localRotation *= Quaternion.FromToRotation(n1, -n2);
var joint = draggedBody.gameObject.AddComponent <ConfigurableJoint>();
joint.autoConfigureConnectedAnchor = false;
joint.anchor = pointOnDraggedBody;
joint.connectedBody = body;
joint.connectedAnchor = body.transform.InverseTransformPoint(worldPos);
joint.enableCollision = true;
var drive = new JointDrive();
drive.maximumForce = 100.0f;
drive.positionDamper = 1.0f;
drive.positionSpring = 100.0f;
joint.xDrive = drive;
joint.yDrive = drive;
joint.zDrive = drive;
joint.slerpDrive = drive;
joint.rotationDriveMode = RotationDriveMode.Slerp;
draggedBody.transform.localRotation = backupRotation;
StartCoroutine(strengthenJoint(joint));
onObjectsAttached(joint, body.gameObject, draggedBody.gameObject);
}
void SetConfig()
{
if (joint == null)
{
return;
}
// set joint limits bsaed on public vars
var lowLimit = new SoftJointLimit();
//lowLimit.limit = -maxFlexAngle;
lowLimit.limit = -maxSteeringAngle;
joint.lowAngularXLimit = lowLimit;
var highLimit = new SoftJointLimit();
//highLimit.limit = maxFlexAngle;
highLimit.limit = maxSteeringAngle;
joint.highAngularXLimit = highLimit;
// steering drive (local X axis is object Y axis)
var xDrive = new JointDrive();
xDrive.positionSpring = steeringSpring;
xDrive.positionDamper = steeringDamper;
xDrive.maximumForce = joint.angularXDrive.maximumForce;
joint.angularXDrive = xDrive;
if (steer)
{
joint.angularXMotion = ConfigurableJointMotion.Limited;
}
else
{
joint.angularXMotion = ConfigurableJointMotion.Locked;
}
// motor drive (local Y axis is X axis)
var yDrive = new JointDrive();
yDrive.maximumForce = maxTorque;
yDrive.positionDamper = yDamper;
yDrive.positionSpring = 0f;
joint.angularYZDrive = yDrive;
configModified = false;
}
public void ControlConfig()
{
int adder = 0;
for (int i = 0; i < limbs; i++)
{
ConfigurableJoint cj = limbObjects[i].GetComponent <ConfigurableJoint>();
float useTorque = torque;
if (limbObjects[i].GetComponent <Leg>().foot == true)
{
//useTorque = 10;
}
useTorque = limbObjects[i].GetComponent <Leg>().force;
JointDrive jd = new JointDrive();
jd.positionDamper = 10;
jd.positionSpring = 100;
jd.maximumForce = 9999999999999;
cj.slerpDrive = jd;
cj.rotationDriveMode = RotationDriveMode.Slerp;
Vector3 rotX = new Vector3(outputs[adder] * useTorque, 0, 0);
Vector3 rotY = Vector3.zero;
Vector3 rotZ = Vector3.zero;
adder++;
if (cj.angularYLimit.limit != 0)
{
rotY = new Vector3(0, outputs[adder] * useTorque, 0);
adder++;
}
if (cj.angularZLimit.limit != 0)
{
rotZ = new Vector3(0, 0, outputs[adder] * useTorque);
adder++;
}
cj.targetAngularVelocity = rotX + rotY + rotZ;
//cj.targetAngularVelocity = new Vector3(outputs[adder] * 100, 0, 0);
}
}
internal void Initialize(float relativeMin)
{
rb = GetComponent <Rigidbody>();
JointDrive jd = new JointDrive
{
positionSpring = 1e+5f,
maximumForce = 1e+5f,
positionDamper = 0f
};
joint = GetComponent <ConfigurableJoint>();
joint.angularXDrive = jd;
connected = joint.connectedBody.transform;
isL2 = joint.axis == Vector3.up;
min = joint.lowAngularXLimit.limit;
max = joint.highAngularXLimit.limit;
relMin = relativeMin;
range = (max - min) / 2f;
mid = min + range;
}
/// <summary>
/// Sets a range of 'magic' values on the given <see cref="ConfigurableJoint"/> in order to setup the joint as a 'hand joint' for VR use.
/// </summary>
/// <param name="joint">The joint.</param>
public static void SetDefaultHandJointSettings(ConfigurableJoint joint)
{
joint.xMotion = joint.yMotion = joint.zMotion = ConfigurableJointMotion.Locked;
joint.angularXMotion = joint.angularYMotion = joint.angularZMotion = ConfigurableJointMotion.Free;
var positionDrive = new JointDrive()
{
positionSpring = 200000,
positionDamper = 1,
};
joint.xDrive = joint.yDrive = joint.zDrive = positionDrive;
joint.rotationDriveMode = RotationDriveMode.Slerp;
joint.slerpDrive = new JointDrive()
{
positionSpring = 200000,
positionDamper = 1,
maximumForce = float.MaxValue
};
}
/// <summary>
/// This function gets called when the legs are being set active or inactive and it will set all the joints releted to the legs and balancing
/// Effected Joints: Pelvis, R_Upper_Leg, R_Lower_Leg, L_Upper_Leg, L_Lower_leg.
/// </summary>
/// <param name="pelvisSpring"></param>
/// <param name="legSpring"></param>
/// <param name="legDampen"></param>
private void SetJointValues(float pelvisSpring, float legSpring, float legDampen)
{
JointDrive jointDrive = new JointDrive();
jointDrive.positionSpring = pelvisSpring;
jointDrive.positionDamper = 0f;
jointDrive.maximumForce = 3.402823e+38f;
pelvis.GetComponent <ConfigurableJoint>().angularXDrive = jointDrive;
pelvis.GetComponent <ConfigurableJoint>().angularYZDrive = jointDrive;
jointDrive.positionSpring = legSpring;
jointDrive.positionDamper = legDampen;
jointDrive.maximumForce = 3.402823e+38f;
foreach (Rigidbody leg in legs)
{
leg.GetComponent <ConfigurableJoint>().angularXDrive = jointDrive;
leg.GetComponent <ConfigurableJoint>().angularYZDrive = jointDrive;
}
}
// ばねの設定など
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 * scale_;
conf.xDrive = drive;
}
if (joint.spring_pos.y != 0f)
{
drive = new JointDrive();
drive.positionSpring = joint.spring_pos.y * scale_;
conf.yDrive = drive;
}
if (joint.spring_pos.z != 0f)
{
drive = new JointDrive();
drive.positionSpring = joint.spring_pos.z * scale_;
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;
}
}
private void createSliderJoint()
{
ConfigurableJoint configurableJoint = pusherGameObject.AddComponent <ConfigurableJoint>();
configurableJoint.connectedBody = machineGameObject.GetComponent <Rigidbody>();
configurableJoint.xMotion = ConfigurableJointMotion.Locked;
configurableJoint.yMotion = ConfigurableJointMotion.Locked;
configurableJoint.zMotion = ConfigurableJointMotion.Limited;
configurableJoint.angularXMotion = ConfigurableJointMotion.Locked;
configurableJoint.angularYMotion = ConfigurableJointMotion.Locked;
configurableJoint.angularZMotion = ConfigurableJointMotion.Locked;
SoftJointLimit softJointLimit = new SoftJointLimit();
softJointLimit.limit = pusherMovementRange / 2.0f;
configurableJoint.linearLimit = softJointLimit;
this.pusherJoint = configurableJoint;
this.jointDrive = new JointDrive();
pullSlider();
}
private static void SetLinearDrive(ConfigurableJoint joint, float mass)
{
float gripStrength = 30000f * mass;
float gripSpeed = 100f * mass;
float maxPower = 700f * mass;
JointDrive jointDrive = joint.xDrive;
jointDrive.positionSpring = gripStrength;
jointDrive.positionDamper = gripSpeed;
jointDrive.maximumForce = maxPower;
joint.xDrive = jointDrive;
jointDrive = joint.yDrive;
jointDrive.positionSpring = gripStrength;
jointDrive.positionDamper = gripSpeed;
jointDrive.maximumForce = maxPower;
joint.yDrive = jointDrive;
jointDrive = joint.zDrive;
jointDrive.positionSpring = gripStrength;
jointDrive.positionDamper = gripSpeed;
jointDrive.maximumForce = maxPower;
joint.zDrive = jointDrive;
}
void Start()
{
mySkeleton = GetComponentInParent <Skeleton>();
limbRb = GetComponent <Rigidbody>();
lastRigidbodyPosition = limbRb.position;
limbJoint = GetComponent <ConfigurableJoint>();
if (limbJoint)
{
Vector3 forward = Vector3.Cross(limbJoint.axis, limbJoint.secondaryAxis);
Vector3 up = limbJoint.secondaryAxis;
localToJointSpace = Quaternion.LookRotation(forward, up);
startLocalRotation = transform.localRotation * localToJointSpace;
jointDrive = limbJoint.slerpDrive;
}
//if (transform.localPosition.x < 0f)
rigidbodyPosToCOM = Quaternion.Inverse(transform.rotation) * (limbRb.worldCenterOfMass - transform.position);
// else
// rigidbodyPosToCOM = Quaternion.Inverse(transform.rotation) /* Quaternion.AngleAxis(180f,Vector3.left)*/ * (limbRb.worldCenterOfMass - transform.position);
if (limbJoint)
{
JointLimited(true);
}
limbRb.collisionDetectionMode = mySkeleton.collisionDetectionMode;
limbRb.useGravity = mySkeleton.useGravity;
limbRb.angularDrag = mySkeleton.angularDrag;
limbRb.drag = mySkeleton.drag;
limbRb.interpolation = RigidbodyInterpolation.None;
limbProfile = SetLimbProfile();
SetLimb();
//Debug.Log(this.name + " is setted");
}
private void FixedUpdate()
{
EnsureInitialized();
if (!(joint == null) && useSpring)
{
float num = (!useTension) ? spring : (maxForce / tensionDist);
float num2 = (!useTension) ? damper : (maxForce / maxSpeed);
Vector3 vector = -new Vector3(target - centerValue, 0f, 0f);
JointDrive xDrive = joint.xDrive;
if (joint.targetPosition != vector || xDrive.maximumForce != maxForce || xDrive.positionSpring != num || xDrive.positionDamper != num2)
{
joint.xDrive = new JointDrive
{
maximumForce = maxForce,
positionSpring = num,
positionDamper = num2
};
joint.targetPosition = vector;
rigid.WakeUp();
}
}
}
void CreateAndConnectMuscles(Transform ragdollBone, Transform animBone)
{
if (ragdollBone.tag == "RagdollBone")
{
Rigidbody boneRb = ragdollBone.GetComponent <Rigidbody>();
if (!boneRb)
{
boneRb = ragdollBone.gameObject.AddComponent <Rigidbody>();
}
boneRb.mass = boneMass;
boneRb.drag = 0.0f;
boneRb.angularDrag = 0.0f;
boneRb.useGravity = useGravity;
Muscle muscle = ragdollBone.gameObject.AddComponent <Muscle>();
muscle.ragdollRoot = transform;
muscle.animatedRigRoot = animatedTargetRigHip;
muscle.SetAnimationTarget(animBone);
Muscle parentMuscle = ragdollBone.parent.GetComponent <Muscle>();
if (parentMuscle)
{
muscle.SetParent(parentMuscle);
JointDrive posMatchingSpring = new JointDrive();
posMatchingSpring.positionSpring = 1000.0f;
posMatchingSpring.positionDamper = 100.0f;
posMatchingSpring.maximumForce = Mathf.Infinity;
muscle.positionMatchingSpring = posMatchingSpring;
}
this.ragdollMuscles.Add(muscle);
for (int i = 0; i < ragdollBone.childCount; i++)
{
Transform childRagdollBone = ragdollBone.GetChild(i);
Transform childAnimBone = animBone.GetChild(i);
CreateAndConnectMuscles(childRagdollBone, childAnimBone);
}
}
}
/// <summary>
/// Create BodyPart object and add it to dictionary.
/// </summary>
public void SetupBodyPart(Transform t)
{
var bp = new BodyPart
{
rb = t.GetComponent <Rigidbody>(),
joint = t.GetComponent <ConfigurableJoint>(),
startingPos = t.position,
startingRot = t.rotation
};
bp.rb.maxAngularVelocity = k_MaxAngularVelocity;
// Add & setup the ground contact script
//bp.groundContact = t.GetComponent<GroundContact>();
//if (!bp.groundContact)
//{
// bp.groundContact = t.gameObject.AddComponent<GroundContact>();
// bp.groundContact.agent = gameObject.GetComponent<Agent>();
//}
//else
//{
// bp.groundContact.agent = gameObject.GetComponent<Agent>();
//}
if (bp.joint)
{
var jd = new JointDrive
{
positionSpring = maxJointSpring,
positionDamper = jointDampen,
maximumForce = maxJointForceLimit
};
bp.joint.slerpDrive = jd;
}
bp.thisJdController = this;
bodyPartsDict.Add(t, bp);
bodyPartsList.Add(bp);
}
static int get_positionDamper(IntPtr L)
{
object o = LuaScriptMgr.GetLuaObject(L, 1);
if (o == null)
{
LuaTypes types = LuaDLL.lua_type(L, 1);
if (types == LuaTypes.LUA_TTABLE)
{
LuaDLL.luaL_error(L, "unknown member name positionDamper");
}
else
{
LuaDLL.luaL_error(L, "attempt to index positionDamper on a nil value");
}
}
JointDrive obj = (JointDrive)o;
LuaScriptMgr.Push(L, obj.positionDamper);
return(1);
}
static int get_mode(IntPtr L)
{
object o = LuaScriptMgr.GetLuaObject(L, 1);
if (o == null)
{
LuaTypes types = LuaDLL.lua_type(L, 1);
if (types == LuaTypes.LUA_TTABLE)
{
LuaDLL.luaL_error(L, "unknown member name mode");
}
else
{
LuaDLL.luaL_error(L, "attempt to index mode on a nil value");
}
}
JointDrive obj = (JointDrive)o;
LuaScriptMgr.PushEnum(L, obj.mode);
return(1);
}
void createCursorJoint()
{
var joint = cursorObject.gameObject.AddComponent <ConfigurableJoint>();
joint.autoConfigureConnectedAnchor = false;
joint.connectedBody = draggedBody;
joint.connectedAnchor = pointOnDraggedBody;
joint.enableCollision = false;
var drive = new JointDrive();
drive.maximumForce = 50.0f;
drive.positionDamper = 1.0f;
drive.positionSpring = 10.0f;
joint.xDrive = drive;
joint.yDrive = drive;
joint.zDrive = drive;
joint.slerpDrive = drive;
joint.rotationDriveMode = RotationDriveMode.Slerp;
this.cursorJoint = joint;
cursorLineRenderer.enabled = true;
}
void CreateJoint(GameObject obj)
{
grabJoint = gameObject.AddComponent <ConfigurableJoint>();
// from fixedjoint
grabJoint.breakForce = 1500f;
grabJoint.connectedBody = obj.GetComponent <Rigidbody>();
// configurablejoint additions - make it behave like a fixed joint
grabJoint.xMotion = ConfigurableJointMotion.Locked;
grabJoint.yMotion = ConfigurableJointMotion.Locked;
grabJoint.zMotion = ConfigurableJointMotion.Locked;
grabJoint.angularXMotion = ConfigurableJointMotion.Locked;
grabJoint.angularYMotion = ConfigurableJointMotion.Locked;
grabJoint.angularZMotion = ConfigurableJointMotion.Locked;
if (!HeldObjectInteractable.RelativeAnchor)
{
grabJoint.autoConfigureConnectedAnchor = false;
grabJoint.connectedAnchor = Vector3.zero;
grabJoint.anchor = Vector3.zero;
grabJoint.angularXMotion = ConfigurableJointMotion.Free;
grabJoint.angularYMotion = ConfigurableJointMotion.Free;
grabJoint.angularZMotion = ConfigurableJointMotion.Free;
JointDrive drive = new JointDrive();
drive.positionSpring = 100f;
drive.positionDamper = 15f;
drive.maximumForce = Mathf.Infinity;
grabJoint.slerpDrive = drive;
grabJoint.rotationDriveMode = RotationDriveMode.Slerp;
grabJoint.configuredInWorldSpace = false;
grabJointStartRotation = MyRigidbody.rotation;
grabJoint.SetTargetRotationLocal(grabJoint.connectedBody.rotation, grabJointStartRotation);
}
}
protected void BeginPhysicalDrag()
{
for (int i = 0; i < _focusObjects.Count; ++i)
{
GameObject focusObject = _focusObjects[i];
foreach (Rigidbody rb in focusObject.transform.GetComponentsInChildren <Rigidbody>(true))
{
rb.isKinematic = false;
// Constrain the dragged object to the stylus.
ConfigurableJoint joint = rb.gameObject.AddComponent <ConfigurableJoint>();
joint.targetPosition = Vector3.zero;
joint.targetRotation = Quaternion.identity;
joint.targetVelocity = Vector3.zero;
joint.targetAngularVelocity = Vector3.zero;
joint.connectedBody = GetComponent <Rigidbody>();
joint.anchor = rb.gameObject.transform.InverseTransformPoint(_contactPoint);
joint.axis = Vector3.right;
joint.secondaryAxis = Vector3.forward;
JointDrive drive = new JointDrive();
drive.mode = JointDriveMode.Position;
drive.maximumForce = 10000.0f;
drive.positionSpring = 100.0f;
drive.positionDamper = 1.0f;
joint.xDrive = drive;
joint.yDrive = drive;
joint.zDrive = drive;
joint.angularXDrive = drive;
joint.angularYZDrive = drive;
_focusJoints[rb.gameObject] = joint;
}
}
}
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);
}
}
/// <summary>
/// Update the values for damping and spring when they have changed
/// </summary>
private void UpdateSpringAndDamping()
{
//return when the joint is null
if (joint == null)
{
return;
}
if ((lastDamping != jointDampingValue) || (lastSpring != jointSpringValue))
{
JointDrive drv = joint.angularXDrive;
drv.positionSpring = jointSpringValue;
drv.positionDamper = jointDampingValue;
joint.angularXDrive = drv;
JointDrive drv2 = joint.angularYZDrive;
drv2.positionSpring = jointSpringValue;
drv2.positionDamper = jointDampingValue;
joint.angularYZDrive = drv2;
lastDamping = jointDampingValue;
lastSpring = jointSpringValue;
}
}
static int set_maximumForce(IntPtr L)
{
object o = LuaScriptMgr.GetLuaObject(L, 1);
if (o == null)
{
LuaTypes types = LuaDLL.lua_type(L, 1);
if (types == LuaTypes.LUA_TTABLE)
{
LuaDLL.luaL_error(L, "unknown member name maximumForce");
}
else
{
LuaDLL.luaL_error(L, "attempt to index maximumForce on a nil value");
}
}
JointDrive obj = (JointDrive)o;
obj.maximumForce = (float)LuaScriptMgr.GetNumber(L, 3);
LuaScriptMgr.SetValueObject(L, 1, obj);
return(0);
}
// Use this for initialization
void Start()
{
sceneCamera = Camera.main;
score = 0;
drive = GetComponent <ConfigurableJoint>().yDrive;
drive.mode = JointDriveMode.Position;
if (isLocalPlayer)
{
if (sceneCamera)
{
DisableRespawnUI();
//Lock the cursor
Cursor.lockState = CursorLockMode.Locked;
}
}
else
{
AssignRemoteLayer();
DisableLocalComponents();
}
}
public void InitializeJoints()
{
if (IsInitialized)
{
return;
}
IsInitialized = true;
_strong = new JointDrive
{
positionSpring = 1000,
positionDamper = 0,
maximumForce = Mathf.Infinity
};
_medium = new JointDrive
{
positionSpring = 600,
positionDamper = 0,
maximumForce = Mathf.Infinity
};
_weak = new JointDrive
{
positionSpring = 100,
positionDamper = 0,
maximumForce = Mathf.Infinity
};
_none = new JointDrive
{
positionSpring = 0,
positionDamper = 0,
maximumForce = Mathf.Infinity
};
}
ConfigurableJoint CreateLink(GameObject attachedBody, Vector3 connectionPoint)
{
//This creates the configurable joint between platfroms that have near edges.
ConfigurableJoint link1 = gameObject.AddComponent <ConfigurableJoint>();
JointDrive springJointDrive = new JointDrive();
springJointDrive.positionSpring = strength;
springJointDrive.positionDamper = dampen;
springJointDrive.maximumForce = 99999.9f;
link1.connectedBody = attachedBody.GetComponent <Rigidbody>();
Vector3 anchorLoc = transform.InverseTransformPoint(connectionPoint);
link1.anchor = anchorLoc;
link1.xDrive = springJointDrive;
link1.yDrive = springJointDrive;
link1.zDrive = springJointDrive;
link1.angularXDrive = springJointDrive;
link1.angularYZDrive = springJointDrive;
link1.xMotion = ConfigurableJointMotion.Locked;
link1.zMotion = ConfigurableJointMotion.Locked;
return(link1);
}
//-------------------------------------------------------------
//--Functions
//-------------------------------------------------------------
//---Player Setup--//
////////////////////
void PlayerSetup()
{
cam = Camera.main;
//Setup joint drives
BalanceOn = new JointDrive();
BalanceOn.positionSpring = balanceStrength;
BalanceOn.positionDamper = 0;
BalanceOn.maximumForce = Mathf.Infinity;
PoseOn = new JointDrive();
PoseOn.positionSpring = limbStrength;
PoseOn.positionDamper = 0;
PoseOn.maximumForce = Mathf.Infinity;
CoreStiffness = new JointDrive();
CoreStiffness.positionSpring = coreStrength;
CoreStiffness.positionDamper = 0;
CoreStiffness.maximumForce = Mathf.Infinity;
ReachStiffness = new JointDrive();
ReachStiffness.positionSpring = armReachStiffness;
ReachStiffness.positionDamper = 0;
ReachStiffness.maximumForce = Mathf.Infinity;
DriveOff = new JointDrive();
DriveOff.positionSpring = 25;
DriveOff.positionDamper = 0;
DriveOff.maximumForce = Mathf.Infinity;
//Setup/reroute active ragdoll parts to array
APR_Parts = new GameObject[]
{
//array index numbers
//0
Root,
//1
Body,
//2
Head,
//3
UpperRightArm,
//4
LowerRightArm,
//5
UpperLeftArm,
//6
LowerLeftArm,
//7
UpperRightLeg,
//8
LowerRightLeg,
//9
UpperLeftLeg,
//10
LowerLeftLeg,
//11
RightFoot,
//12
LeftFoot
};
//Setup original pose for joint drives
BodyTarget = APR_Parts[1].GetComponent <ConfigurableJoint>().targetRotation;
HeadTarget = APR_Parts[2].GetComponent <ConfigurableJoint>().targetRotation;
UpperRightArmTarget = APR_Parts[3].GetComponent <ConfigurableJoint>().targetRotation;
LowerRightArmTarget = APR_Parts[4].GetComponent <ConfigurableJoint>().targetRotation;
UpperLeftArmTarget = APR_Parts[5].GetComponent <ConfigurableJoint>().targetRotation;
LowerLeftArmTarget = APR_Parts[6].GetComponent <ConfigurableJoint>().targetRotation;
UpperRightLegTarget = APR_Parts[7].GetComponent <ConfigurableJoint>().targetRotation;
LowerRightLegTarget = APR_Parts[8].GetComponent <ConfigurableJoint>().targetRotation;
UpperLeftLegTarget = APR_Parts[9].GetComponent <ConfigurableJoint>().targetRotation;
LowerLeftLegTarget = APR_Parts[10].GetComponent <ConfigurableJoint>().targetRotation;
}
public void LoadBoomHead()
{
if (savedSectionsLocalPos.Count > 0)
{
//Reset section(s) to org pos
foreach (KeyValuePair <int, SectionInfo> section in sections)
{
section.Value.transform.position = this.part.transform.TransformPoint(section.Value.orgLocalPos);
}
}
KAS_Shared.DebugLog("LoadBoomHead(TelescopicArm) Create physical object...");
boomHeadPhysicModule = this.part.gameObject.GetComponent <KASModulePhysicChild>();
if (!boomHeadPhysicModule)
{
KAS_Shared.DebugLog("LoadBoomHead(TelescopicArm) - KASModulePhysicChild do not exist, adding it...");
boomHeadPhysicModule = this.part.gameObject.AddComponent <KASModulePhysicChild>();
}
boomHeadPhysicModule.mass = boomHeadMass;
boomHeadPhysicModule.physicObj = sections[0].transform.gameObject;
boomHeadPhysicModule.Start();
orgBoomHeadMass = this.part.mass;
float newMass = this.part.mass - boomHeadMass;
if (newMass > 0)
{
this.part.mass = newMass;
}
else
{
KAS_Shared.DebugWarning("LoadBoomHead(TelescopicArm) - Mass of the boom head is greater than the part !");
}
KAS_Shared.DebugLog("LoadRotor - Disable collision...");
disableSectionCollision();
KAS_Shared.DebugLog("LoadBoomHead(TelescopicArm) - Create configurable joint...");
slideJoint = this.part.gameObject.AddComponent <ConfigurableJoint>();
slideJoint.connectedBody = sections[0].transform.rigidbody;
slideJoint.axis = Vector3.zero;
slideJoint.secondaryAxis = Vector3.zero;
slideJoint.breakForce = breakForce;
slideJoint.breakTorque = breakForce;
slideJoint.angularXMotion = ConfigurableJointMotion.Locked;
slideJoint.angularYMotion = ConfigurableJointMotion.Locked;
slideJoint.angularZMotion = ConfigurableJointMotion.Locked;
slideJoint.xMotion = ConfigurableJointMotion.Locked;
slideJoint.yMotion = ConfigurableJointMotion.Locked;
slideJoint.zMotion = ConfigurableJointMotion.Locked;
if (direction.x != 0)
{
slideJoint.xMotion = ConfigurableJointMotion.Limited;
driveWay = direction.x;
KAS_Shared.DebugLog("LoadBoomHead(TelescopicArm) - Direction set to x axis with driveWay set to : " + driveWay);
}
else if (direction.y != 0)
{
slideJoint.yMotion = ConfigurableJointMotion.Limited;
driveWay = direction.y;
KAS_Shared.DebugLog("LoadBoomHead(TelescopicArm) - Direction set to y axis with driveWay set to : " + driveWay);
}
else if (direction.z != 0)
{
slideJoint.zMotion = ConfigurableJointMotion.Limited;
driveWay = direction.z;
KAS_Shared.DebugLog("LoadBoomHead(TelescopicArm) - Direction set to z axis with driveWay set to : " + driveWay);
}
JointDrive drv = new JointDrive();
drv.mode = JointDriveMode.PositionAndVelocity;
drv.positionSpring = driveSpring;
drv.positionDamper = driveDamper;
drv.maximumForce = driveForce;
slideJoint.xDrive = slideJoint.yDrive = slideJoint.zDrive = drv;
SoftJointLimit jointLimit = new SoftJointLimit();
jointLimit.limit = sectionTotalLenght;
jointLimit.damper = 200;
jointLimit.spring = 1000;
jointLimit.bounciness = 1;
slideJoint.linearLimit = jointLimit;
if (savedSectionsLocalPos.Count > 0)
{
KAS_Shared.DebugLog("LoadBoomHead(TelescopicArm) - Re-set section position from save");
float sumLenght = 0;
foreach (KeyValuePair <int, SectionInfo> section in sections)
{
KAS_Shared.DebugLog("LoadBoomHead(TelescopicArm) - Move section " + section.Key + " to local position : " + section.Value.savedLocalPos);
section.Value.transform.position = this.part.transform.TransformPoint(section.Value.savedLocalPos);
sumLenght += section.Value.lenght;
if (headPos > sumLenght)
{
KAS_Shared.DebugLog("LoadBoomHead(TelescopicArm) - Parent section " + sections[section.Key + 1].transform.name + " to : " + section.Value.transform.name);
sections[section.Key + 1].transform.parent = section.Value.transform;
}
}
}
// Get boom head attach nodes
attachNodes.Clear();
ConfigNode node = KAS_Shared.GetBaseConfigNode(this);
List <string> attachNodesSt = new List <string>(node.GetValues("attachNode"));
foreach (String anString in attachNodesSt)
{
AttachNode an = this.part.findAttachNode(anString);
if (an != null)
{
KAS_Shared.DebugLog("LoadBoomHead(TelescopicArm) - Boom head attach node added : " + an.id);
attachNodes.Add(an);
if (an.attachedPart)
{
KAS_Shared.DebugLog("LoadBoomHead(TelescopicArm) - Setting boom head joint on : " + an.attachedPart.partInfo.title);
KAS_Shared.RemoveFixedJointBetween(this.part, an.attachedPart);
KAS_Shared.RemoveHingeJointBetween(this.part, an.attachedPart);
FixedJoint fjnt = an.attachedPart.gameObject.AddComponent <FixedJoint>();
fjnt.connectedBody = sections[0].transform.rigidbody;
fixedJnts.Add(fjnt);
}
}
}
SetTargetPos(targetPos);
boomHeadLoaded = true;
}
// ばねの設定など
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;
}
}
private void DisconnectMuscle(Muscle m, bool sever, bool deactivate)
{
m.state.pinWeightMlp = 0f;
m.state.muscleWeightMlp = 0f;
m.state.muscleDamperAdd = 0f;
m.state.muscleDamperMlp = 0f;
m.state.mappingWeightMlp = 0f;
m.state.maxForceMlp = 0f;
m.state.immunity = 0f;
m.state.impulseMlp = 1f;
if (sever)
{
m.joint.xMotion = ConfigurableJointMotion.Free;
m.joint.yMotion = ConfigurableJointMotion.Free;
m.joint.zMotion = ConfigurableJointMotion.Free;
m.IgnoreAngularLimits(true);
if (!hierarchyIsFlat)
{
m.joint.transform.parent = transform;
}
}
else
{
m.IgnoreAngularLimits(false);
}
bool applyMappedVelocity = !m.joint.gameObject.activeInHierarchy || m.rigidbody.isKinematic;
if (activeState == State.Frozen)
{
applyMappedVelocity = false;
}
// In case disconnecting in disabled mode
if (!m.joint.gameObject.activeInHierarchy && !deactivate)
{
m.MoveToTarget();
m.joint.gameObject.SetActive(true);
}
m.SetKinematic(false);
JointDrive slerpDrive = new JointDrive();
slerpDrive.positionSpring = 0f;
slerpDrive.maximumForce = 0f;
slerpDrive.positionDamper = 0f;
m.joint.slerpDrive = slerpDrive;
// Enable internal collisions with the disconnected muscle
if (!deactivate)
{
for (int i = 0; i < muscles.Length; i++)
{
if (muscles[i] != m && !muscles[i].state.isDisconnected)
{
foreach (Collider c1 in m.colliders)
{
foreach (Collider c2 in muscles[i].colliders)
{
if (c1.enabled && c2.enabled)
{
Physics.IgnoreCollision(c1, c2, false);
}
}
}
}
}
if (applyMappedVelocity)
{
m.rigidbody.velocity = m.mappedVelocity;
m.rigidbody.angularVelocity = m.mappedAngularVelocity;
}
}
else
{
m.joint.gameObject.SetActive(false);
}
if (m.isPropMuscle)
{
var propMuscle = m.joint.GetComponent <PropMuscle>();
if (propMuscle.activeProp != null)
{
propMuscle.currentProp = null;
}
}
m.state.isDisconnected = true;
foreach (BehaviourBase b in behaviours)
{
b.OnMuscleDisconnected(m);
}
if (OnMuscleDisconnected != null)
{
OnMuscleDisconnected(m);
}
}
private void INTERNAL_get_zDrive(out JointDrive value){}
private void INTERNAL_set_zDrive(ref JointDrive value){}
private void INTERNAL_get_angularXDrive(out JointDrive value){}
private void INTERNAL_set_angularXDrive(ref JointDrive value){}
/// <summary>
/// Crenates the proper joint type for this node.
/// </summary>
public void CreateJoint()
{
if (joint != null || GetSkeletalJoint() == null)
{
return;
}
//this is the conditional for Identifying wheels
if (GetSkeletalJoint().GetJointType() == SkeletalJointType.ROTATIONAL)
{
if (this.HasDriverMeta <WheelDriverMeta>())
{
OrientWheelNormals();
}
RotationalJoint_Base nodeR = (RotationalJoint_Base)GetSkeletalJoint();
//takes the x, y, and z axis information from a custom vector class to unity's vector class
joint = ConfigJointInternal <HingeJoint>(nodeR.basePoint.AsV3(), nodeR.axis.AsV3(), delegate(HingeJoint jointSub)
{
jointSub.useLimits = nodeR.hasAngularLimit;
if (nodeR.hasAngularLimit)
{
AngularLimit(jointSub,
MathfExt.ToDegrees(nodeR.currentAngularPosition),
MathfExt.ToDegrees(nodeR.angularLimitLow),
MathfExt.ToDegrees(nodeR.angularLimitHigh));
}
});
//don't worry, I'm a doctor
if (this.HasDriverMeta <WheelDriverMeta>())
{
CreateWheel();
}
}
else if (GetSkeletalJoint().GetJointType() == SkeletalJointType.CYLINDRICAL)
{
CylindricalJoint_Base nodeC = (CylindricalJoint_Base)GetSkeletalJoint();
joint = ConfigJointInternal <ConfigurableJoint>(nodeC.basePoint.AsV3(), nodeC.axis.AsV3(), delegate(ConfigurableJoint jointSub)
{
jointSub.xMotion = ConfigurableJointMotion.Limited;
jointSub.angularXMotion = !nodeC.hasAngularLimit ? ConfigurableJointMotion.Free : ConfigurableJointMotion.Limited;
LinearLimit(jointSub,
nodeC.currentLinearPosition,
nodeC.linearLimitStart,
nodeC.linearLimitEnd);
if (GetSkeletalJoint().cDriver != null && GetSkeletalJoint().cDriver.GetDriveType().IsPneumatic())
{
JointDrive drMode = new JointDrive();
drMode.mode = JointDriveMode.Velocity;
drMode.maximumForce = 100.0f;
jointSub.xDrive = drMode;
}
if (jointSub.angularXMotion == ConfigurableJointMotion.Limited)
{
AngularLimit(jointSub,
MathfExt.ToDegrees(nodeC.currentAngularPosition),
MathfExt.ToDegrees(nodeC.angularLimitLow),
MathfExt.ToDegrees(nodeC.angularLimitHigh));
}
});
}
else if (GetSkeletalJoint().GetJointType() == SkeletalJointType.LINEAR)
{
LinearJoint_Base nodeL = (LinearJoint_Base)GetSkeletalJoint();
joint = ConfigJointInternal <ConfigurableJoint>(nodeL.basePoint.AsV3(), nodeL.axis.AsV3(), delegate(ConfigurableJoint jointSub)
{
jointSub.xMotion = ConfigurableJointMotion.Limited;
LinearLimit(jointSub,
nodeL.currentLinearPosition,
nodeL.linearLimitLow,
nodeL.linearLimitHigh);
});
//TODO make code good
if (GetSkeletalJoint().cDriver != null)
{
if (GetSkeletalJoint().cDriver.GetDriveType().IsElevator())
{
Debug.Log(GetSkeletalJoint().cDriver.portA);
unityObject.AddComponent <ElevatorScript>();
unityObject.GetComponent <ElevatorScript>().eType = (ElevatorType)this.GetDriverMeta <ElevatorDriverMeta>().type;
Debug.Log("added");
}
}
}
SetXDrives();
}
private void INTERNAL_get_rotationDrive(out JointDrive value){}
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());
}
}
private void INTERNAL_set_rotationDrive(ref JointDrive value){}
