protected virtual bool IsJointAxisDefined()
{
UnityEngine.Joint joint = GetComponent<UnityEngine.Joint>();
return !(Math.Abs(joint.axis.x) < Tolerance &&
Math.Abs(joint.axis.y) < Tolerance &&
Math.Abs(joint.axis.z) < Tolerance);
}
static public int constructor(IntPtr l)
{
UnityEngine.Joint o;
o = new UnityEngine.Joint();
pushObject(l, o);
return(1);
}
private static UrdfJoint AddCorrectJointType(GameObject linkObject, JointTypes jointType)
{
UrdfJoint urdfJoint = null;
switch (jointType)
{
case JointTypes.Fixed:
urdfJoint = UrdfJointFixed.Create(linkObject);
break;
case JointTypes.Continuous:
urdfJoint = UrdfJointContinuous.Create(linkObject);
break;
case JointTypes.Revolute:
urdfJoint = UrdfJointRevolute.Create(linkObject);
break;
case JointTypes.Floating:
urdfJoint = UrdfJointFloating.Create(linkObject);
break;
case JointTypes.Prismatic:
urdfJoint = UrdfJointPrismatic.Create(linkObject);
break;
case JointTypes.Planar:
urdfJoint = UrdfJointPlanar.Create(linkObject);
break;
}
UnityEngine.Joint unityJoint = linkObject.GetComponent<UnityEngine.Joint>();
unityJoint.connectedBody = linkObject.transform.parent.gameObject.GetComponent<Rigidbody>();
unityJoint.autoConfigureConnectedAnchor = true;
return urdfJoint;
}
private bool IsAnchorTransformed() // TODO : Check for tolerances before implementation
{
UnityEngine.Joint joint = GetComponent <UnityEngine.Joint>();
return(Math.Abs(joint.anchor.x) > Tolerance ||
Math.Abs(joint.anchor.x) > Tolerance ||
Math.Abs(joint.anchor.x) > Tolerance);
}
static public int set_connectedBody(IntPtr l)
{
UnityEngine.Joint o = (UnityEngine.Joint)checkSelf(l);
UnityEngine.Rigidbody v;
checkType(l, 2, out v);
o.connectedBody = v;
return(0);
}
static public int set_axis(IntPtr l)
{
UnityEngine.Joint o = (UnityEngine.Joint)checkSelf(l);
UnityEngine.Vector3 v;
checkType(l, 2, out v);
o.axis = v;
return(0);
}
static public int set_connectedAnchor(IntPtr l)
{
UnityEngine.Joint o = (UnityEngine.Joint)checkSelf(l);
UnityEngine.Vector3 v;
checkType(l, 2, out v);
o.connectedAnchor = v;
return(0);
}
private bool IsAnchorTransformed()
{
UnityEngine.Joint joint = GetComponent<UnityEngine.Joint>();
return Math.Abs(joint.anchor.x) > Tolerance ||
Math.Abs(joint.anchor.x) > Tolerance ||
Math.Abs(joint.anchor.x) > Tolerance;
}
/** Disconnects the bone from the joint. */
public void Disconnect(Bone bone)
{
UnityEngine.Joint joint = joints[bone];
Destroy(joint);
if (!iterating)
{
joints.Remove(bone);
}
}
void Start () {
unit = gameObject.GetUnit ();
behavior = gameObject.FindBehavior () as CharacterBehavior;
//behavior = unit.GetComponent<CharacterBahavior> ();
// ---
thisRigidbody = GetComponent <Rigidbody> ();
thisCollider = GetComponent <Collider> ();
joint = GetComponent <Joint> ();
}
static public int set_autoConfigureConnectedAnchor(IntPtr l)
{
UnityEngine.Joint o = (UnityEngine.Joint)checkSelf(l);
bool v;
checkType(l, 2, out v);
o.autoConfigureConnectedAnchor = v;
return(0);
}
static public int set_breakTorque(IntPtr l)
{
UnityEngine.Joint o = (UnityEngine.Joint)checkSelf(l);
float v;
checkType(l, 2, out v);
o.breakTorque = v;
return(0);
}
static public int set_enableCollision(IntPtr l)
{
UnityEngine.Joint o = (UnityEngine.Joint)checkSelf(l);
bool v;
checkType(l, 2, out v);
o.enableCollision = v;
return(0);
}
public static void TryToMakeNewConnection(Joint from, Joint to, Connection lastDraggedConnection)
{
if (from != null && to != null){
if (lastDraggedConnection.InputJoint != null && lastDraggedConnection.OutputJoint != null){
ObjectConnector.UpdateExistingConnection(from, to, lastDraggedConnection);
} else {
ObjectConnector.CreateNewConnection(from, to);
}
}
}
public static float DrawJointLimit(Joint joint, string label, Vector3 axis, float limit, Color color, float openValue, bool drawHandles)
{
if (Event.current.type == EventType.mouseDown) isDragging = false;
if (Event.current.type == EventType.mouseUp) isDragging = false;
float radius = HandleUtility.GetHandleSize(joint.transform.position);
Vector3 center = joint.transform.TransformPoint(joint.anchor);
Quaternion axisOffset = Quaternion.AngleAxis(GetViewOffset(joint, axis), axis);
Vector3 limitAxis = joint.transform.TransformDirection(axisOffset * axis).normalized;
Vector3 limitAxisOrtho = joint.transform.TransformDirection(axisOffset * new Vector3(axis.z, axis.x, axis.y)).normalized;
Vector3 limitAxisCross = Vector3.Cross(limitAxis, limitAxisOrtho);
Handles.color = color;
Handles.DrawSolidArc(center, limitAxis, limitAxisCross, -limit, radius);
Handles.color = new Color(color.r, color.g, color.b, 1f);
GUI.color = new Color(color.r, color.g, color.b, 1f);
//if (limitAxis != Vector3.zero) Handles.CircleCap(0, center, Quaternion.LookRotation(limitAxis), radius);
Quaternion angleAxis = Quaternion.AngleAxis(-limit, limitAxis);
Vector3 handleVector = angleAxis * limitAxisCross;
Quaternion handleRotation = Quaternion.AngleAxis(-limit, limitAxis);
if (limitAxisCross != Vector3.zero && limitAxisOrtho != Vector3.zero) {
handleRotation = Quaternion.AngleAxis(-limit, limitAxis) * Quaternion.LookRotation(limitAxisCross) * Quaternion.LookRotation(limitAxisOrtho);
}
if (!drawHandles) return limit;
float newLimit = Handles.ScaleValueHandle(limit, center + handleVector * radius, Quaternion.identity, radius, Handles.SphereCap, 1);
//float newLimit = Handles.han(handleRotation, center + handleVector * radius, limit);
string labelInfo = label;
if (newLimit == 0) {
labelInfo = "Open " + label;
if (Handles.Button(center + handleVector * radius, handleRotation, radius * 0.2f, radius * 0.07f, Handles.SphereCap)) {
newLimit = openValue;
}
}
Handles.Label(center + handleVector * radius * 1.2f, labelInfo);
if (newLimit != limit) {
if (!isDragging) {
Undo.RecordObject(joint, "Change Joint Limits");
isDragging = true;
}
}
GUI.color = Color.white;
return newLimit;
}
static public int get_connectedMassScale(IntPtr l)
{
try {
UnityEngine.Joint self = (UnityEngine.Joint)checkSelf(l);
pushValue(l, self.connectedMassScale);
return(1);
}
catch (Exception e) {
return(error(l, e));
}
}
static public int get_anchor(IntPtr l)
{
try {
UnityEngine.Joint self = (UnityEngine.Joint)checkSelf(l);
pushValue(l, self.anchor);
return(1);
}
catch (Exception e) {
return(error(l, e));
}
}
static public int get_enablePreprocessing(IntPtr l)
{
try {
UnityEngine.Joint self = (UnityEngine.Joint)checkSelf(l);
pushValue(l, self.enablePreprocessing);
return(1);
}
catch (Exception e) {
return(error(l, e));
}
}
static public int get_enableCollision(IntPtr l)
{
try {
UnityEngine.Joint self = (UnityEngine.Joint)checkSelf(l);
pushValue(l, true);
pushValue(l, self.enableCollision);
return(2);
}
catch (Exception e) {
return(error(l, e));
}
}
static public int constructor(IntPtr l)
{
try {
UnityEngine.Joint o;
o = new UnityEngine.Joint();
pushValue(l, o);
return(1);
}
catch (Exception e) {
return(error(l, e));
}
}
static public int get_currentTorque(IntPtr l)
{
try {
UnityEngine.Joint self = (UnityEngine.Joint)checkSelf(l);
pushValue(l, true);
pushValue(l, self.currentTorque);
return(2);
}
catch (Exception e) {
return(error(l, e));
}
}
static public int get_autoConfigureConnectedAnchor(IntPtr l)
{
try {
UnityEngine.Joint self = (UnityEngine.Joint)checkSelf(l);
pushValue(l, true);
pushValue(l, self.autoConfigureConnectedAnchor);
return(2);
}
catch (Exception e) {
return(error(l, e));
}
}
static public int constructor(IntPtr l) {
try {
UnityEngine.Joint o;
o=new UnityEngine.Joint();
pushValue(l,true);
pushValue(l,o);
return 2;
}
catch(Exception e) {
return error(l,e);
}
}
static public int get_anchor(IntPtr l)
{
try {
UnityEngine.Joint self = (UnityEngine.Joint)checkSelf(l);
pushValue(l, self.anchor);
return(1);
}
catch (Exception e) {
LuaDLL.luaL_error(l, e.ToString());
return(0);
}
}
static public int set_anchor(IntPtr l)
{
try {
UnityEngine.Joint self = (UnityEngine.Joint)checkSelf(l);
UnityEngine.Vector3 v;
checkType(l, 2, out v);
self.anchor = v;
return(0);
}
catch (Exception e) {
return(error(l, e));
}
}
static public int constructor(IntPtr l)
{
LuaDLL.lua_remove(l, 1);
UnityEngine.Joint o;
if (matchType(l, 1))
{
o = new UnityEngine.Joint();
pushObject(l, o);
return(1);
}
LuaDLL.luaL_error(l, "New object failed.");
return(0);
}
static public int set_connectedBody(IntPtr l)
{
try {
UnityEngine.Joint self = (UnityEngine.Joint)checkSelf(l);
UnityEngine.Rigidbody v;
checkType(l, 2, out v);
self.connectedBody = v;
return(0);
}
catch (Exception e) {
return(error(l, e));
}
}
static public int set_enablePreprocessing(IntPtr l)
{
try {
UnityEngine.Joint self = (UnityEngine.Joint)checkSelf(l);
bool v;
checkType(l, 2, out v);
self.enablePreprocessing = v;
return(0);
}
catch (Exception e) {
return(error(l, e));
}
}
static public int set_breakTorque(IntPtr l)
{
try {
UnityEngine.Joint self = (UnityEngine.Joint)checkSelf(l);
float v;
checkType(l, 2, out v);
self.breakTorque = v;
return(0);
}
catch (Exception e) {
return(error(l, e));
}
}
static public int set_autoConfigureConnectedAnchor(IntPtr l)
{
try {
UnityEngine.Joint self = (UnityEngine.Joint)checkSelf(l);
bool v;
checkType(l, 2, out v);
self.autoConfigureConnectedAnchor = v;
return(0);
}
catch (Exception e) {
return(error(l, e));
}
}
public static int constructor(IntPtr l)
{
try {
UnityEngine.Joint o;
o=new UnityEngine.Joint();
pushValue(l,o);
return 1;
}
catch(Exception e) {
LuaDLL.luaL_error(l, e.ToString());
return 0;
}
}
static public int set_connectedMassScale(IntPtr l)
{
try {
UnityEngine.Joint self = (UnityEngine.Joint)checkSelf(l);
float v;
checkType(l, 2, out v);
self.connectedMassScale = v;
return(0);
}
catch (Exception e) {
return(error(l, e));
}
}
static public int constructor(IntPtr l)
{
try {
UnityEngine.Joint o;
o = new UnityEngine.Joint();
pushValue(l, o);
return(1);
}
catch (Exception e) {
LuaDLL.luaL_error(l, e.ToString());
return(0);
}
}
static public int set_axis(IntPtr l)
{
try {
UnityEngine.Joint self = (UnityEngine.Joint)checkSelf(l);
UnityEngine.Vector3 v;
checkType(l, 2, out v);
self.axis = v;
pushValue(l, true);
return(1);
}
catch (Exception e) {
return(error(l, e));
}
}
static public int set_massScale(IntPtr l)
{
try {
UnityEngine.Joint self = (UnityEngine.Joint)checkSelf(l);
float v;
checkType(l, 2, out v);
self.massScale = v;
pushValue(l, true);
return(1);
}
catch (Exception e) {
return(error(l, e));
}
}
// Constructor
public Rigidbone (Rigidbody r) {
this.r = r;
t = r.transform;
joint = t.GetComponent<Joint>();
collider = t.GetComponent<Collider>();
if (joint != null) {
c = joint.connectedBody;
updateAnchor = c != null;
}
lastPosition = t.position;
lastRotation = t.rotation;
}
static void ClearReffToAnotherObject(Joint joint)
{
switch (joint.JointType){
case JointType.OneWay_OUT:
joint.SerializedProperty.objectReferenceValue = null;
joint.SerializedProperty.serializedObject.ApplyModifiedProperties();
GUI.changed = true;
break;
case JointType.OneWay_IN:
break;//do nothing
default:
Debug.LogError("Unsuported type "+joint.JointType) ;
break;
}
}
public static void UpdateExistingConnection(Joint jointFrom, Joint jointTo, Connection existingConnection)
{
if (jointFrom == jointTo){
return;
}
Joint changeJointFrom, changeJointTo;
Joint anotherJoint;
if (existingConnection.InputJoint == jointFrom || existingConnection.InputJoint == jointTo){
changeJointFrom = existingConnection.InputJoint;
anotherJoint = existingConnection.OutputJoint;
} else {
changeJointFrom = existingConnection.OutputJoint;
anotherJoint = existingConnection.InputJoint;
}
ClearReffToAnotherObject(changeJointFrom);
changeJointTo = jointFrom == changeJointFrom ? jointTo : jointFrom;
CreateNewConnection(changeJointTo, anotherJoint);
}
public static void CreateNewConnection(Joint joint1, Joint joint2)
{
JointType type1 = joint1.JointType;
JointType type2 = joint2.JointType;
if (CouldConnectTwoJointsWithTypes(type1,type2)){
//anonimous connections;
if (type1 == JointType.OneWay_IN){
joint2.SerializedProperty.objectReferenceValue = joint1.ObjectRefferenceValue;
joint2.SerializedProperty.serializedObject.ApplyModifiedProperties();
} else {
joint1.SerializedProperty.objectReferenceValue = joint2.ObjectRefferenceValue;
joint1.SerializedProperty.serializedObject.ApplyModifiedProperties();
}
GUI.changed = true;
}
}
private void CreateJoint(GameObject obj)
{
var objectScript = obj.GetComponent<VRTK_InteractableObject>();
if (objectScript.grabAttachMechanic == VRTK_InteractableObject.GrabAttachType.Fixed_Joint)
{
controllerAttachJoint = obj.AddComponent<FixedJoint>();
}
else if (objectScript.grabAttachMechanic == VRTK_InteractableObject.GrabAttachType.Spring_Joint)
{
SpringJoint tempSpringJoint = obj.AddComponent<SpringJoint>();
tempSpringJoint.spring = objectScript.springJointStrength;
tempSpringJoint.damper = objectScript.springJointDamper;
if (objectScript.precisionSnap)
{
tempSpringJoint.anchor = obj.transform.InverseTransformPoint(controllerAttachPoint.position);
}
controllerAttachJoint = tempSpringJoint;
}
controllerAttachJoint.breakForce = (objectScript.isDroppable ? objectScript.detachThreshold : Mathf.Infinity);
controllerAttachJoint.connectedBody = controllerAttachPoint;
}
private Rigidbody ReleaseAttachedObjectFromController(bool withThrow)
{
var jointGameObject = controllerAttachJoint.gameObject;
var rigidbody = jointGameObject.GetComponent<Rigidbody>();
if (withThrow)
{
Object.DestroyImmediate(controllerAttachJoint);
}
else
{
Object.Destroy(controllerAttachJoint);
}
controllerAttachJoint = null;
return rigidbody;
}
private void CreateJoint(GameObject obj)
{
var objectScript = obj.GetComponent<VRTK_InteractableObject>();
if (objectScript.grabAttachMechanic == VRTK_InteractableObject.GrabAttachType.Fixed_Joint)
{
controllerAttachJoint = obj.AddComponent<FixedJoint>();
}
else if (objectScript.grabAttachMechanic == VRTK_InteractableObject.GrabAttachType.Spring_Joint)
{
SpringJoint tempSpringJoint = obj.AddComponent<SpringJoint>();
tempSpringJoint.spring = objectScript.springJointStrength;
tempSpringJoint.damper = objectScript.springJointDamper;
controllerAttachJoint = tempSpringJoint;
}
controllerAttachJoint.breakForce = objectScript.detachThreshold;
controllerAttachJoint.connectedBody = controllerAttachPoint;
}
private static Axis GetViewAxis(Joint joint)
{
if (joint.connectedBody == null) return Axis.Z;
CapsuleCollider capsule = joint.GetComponent<CapsuleCollider>();
if (capsule != null && capsule.center != Vector3.zero) return AxisTools.ToAxis(capsule.center);
else {
BoxCollider box = joint.GetComponent<BoxCollider>();
if (box != null && box.center != Vector3.zero) return AxisTools.ToAxis(box.center);
}
return AxisTools.GetAxisToPoint(joint.transform, joint.connectedBody.worldCenterOfMass);
}
private static float GetViewOffset(Joint joint, Vector3 axis)
{
if (joint.connectedBody == null) return 0;
Vector3 directionAxis = joint.transform.right;
Axis viewAxis = GetViewAxis(joint);
switch(viewAxis) {
case Axis.Y: directionAxis = joint.transform.up; break;
case Axis.Z: directionAxis = joint.transform.forward; break;
}
if (FlipAxis(joint, viewAxis)) directionAxis = -directionAxis;
Vector3 axisWorld = joint.transform.TransformDirection(axis).normalized;
Vector3 axisWorldOrtho = joint.transform.TransformDirection(new Vector3(axis.z, axis.x, axis.y)).normalized;
float dot = Vector3.Dot(axisWorldOrtho, directionAxis);
float dot2 = Vector3.Dot(axisWorldOrtho, Vector3.Cross(directionAxis, axisWorld));
if (dot >= 0.5f) return -90;
if (dot2 >= 0.5f) return 0;
if (dot < -0.5f) return 90;
if (dot2 < -0.5f) return 180;
return 0;
}
private static bool FlipAxis(Joint joint, Axis axis)
{
Vector3 direction = Vector3.Normalize(joint.transform.position - joint.connectedBody.worldCenterOfMass);
CapsuleCollider capsuleCollider = joint.GetComponent<CapsuleCollider>();
if (capsuleCollider != null) {
direction = Vector3.Normalize(joint.transform.position - (joint.transform.position - (joint.transform.rotation * capsuleCollider.center)));
}
switch(axis) {
case Axis.X:
return Vector3.Dot(joint.transform.right, direction) < 0;
case Axis.Y:
return Vector3.Dot(joint.transform.up, direction) < 0;
default: return Vector3.Dot(joint.transform.forward, direction) < 0;
}
}
// Common to all joints
private static void ConvertJoint(ref ConfigurableJoint conf, Joint src)
{
conf.anchor = src.anchor;
conf.autoConfigureConnectedAnchor = src.autoConfigureConnectedAnchor;
conf.axis = src.axis;
conf.breakForce = src.breakForce;
conf.breakTorque = src.breakTorque;
conf.connectedAnchor = src.connectedAnchor;
conf.connectedBody = src.connectedBody;
conf.enableCollision = src.enableCollision;
}
public static void InvertSecondaryAxis(ref Joint joint)
{
if (joint is ConfigurableJoint) {
var j = joint as ConfigurableJoint;
Undo.RecordObject(j, "Invert Secondary Axis");
j.secondaryAxis = -j.secondaryAxis;
}
if (joint is CharacterJoint) {
var j = joint as CharacterJoint;
Undo.RecordObject(j, "Invert Swing Axis");
j.swingAxis = -j.swingAxis;
}
}
public static void InvertAxis(ref Joint joint)
{
Undo.RecordObject(joint, "Invert Axis");
joint.axis = -joint.axis;
}
public static Vector3 GetLowXAxisWorld(Joint joint)
{
return joint.transform.rotation * joint.axis;
}
public static HumanBodyBones ToUnityBone(Joint joint)
{
switch (joint)
{
case Joint.ROOT:
return HumanBodyBones.Hips;
case Joint.SPINE:
return HumanBodyBones.Spine;
case Joint.NECK:
return HumanBodyBones.Neck;
case Joint.SHOULDER_LEFT:
return HumanBodyBones.LeftUpperArm;
case Joint.ELBOW_LEFT:
return HumanBodyBones.LeftLowerArm;
case Joint.WRIST_LEFT:
return HumanBodyBones.LeftHand;
case Joint.SHOULDER_RIGHT:
return HumanBodyBones.RightUpperArm;
case Joint.ELBOW_RIGHT:
return HumanBodyBones.RightLowerArm;
case Joint.WRIST_RIGHT:
return HumanBodyBones.RightHand;
case Joint.HIP_LEFT:
return HumanBodyBones.LeftUpperLeg;
case Joint.KNEE_LEFT:
return HumanBodyBones.LeftLowerLeg;
case Joint.ANKLE_LEFT:
return HumanBodyBones.LeftFoot;
case Joint.HIP_RIGHT:
return HumanBodyBones.RightUpperLeg;
case Joint.KNEE_RIGHT:
return HumanBodyBones.RightLowerLeg;
case Joint.ANKLE_RIGHT:
return HumanBodyBones.RightFoot;
}
throw new System.Exception("Joint type " + joint.ToString() + " is not handled.");
}
void HandleDraggConnections(List<Node> nodes, Connection lastDraggedConnection)
{
switch(Event.current.GetTypeForControl(ControlID)){
case EventType.MouseDown:
{
StartDraggJoint = GetJointUnderMousePosition(nodes);
if (StartDraggJoint != null){
GUIUtility.hotControl = StartDraggJoint.ControlID;
Event.current.Use();
}
break;
}
case EventType.mouseUp:
{
if (StartDraggJoint != null){
Joint EndDragJoint = GetJointUnderMousePosition(nodes);
ObjectConnector.TryToMakeNewConnection(StartDraggJoint, EndDragJoint, lastDraggedConnection);
StartDraggJoint = null;
GUIUtility.hotControl = 0;
Event.current.Use();
}
break ;
}
}
}
