Neodroid  0.2.0
Machine Learning Environment Prototyping Tool
JointFix.cs
Go to the documentation of this file.
1 using System;
2 using UnityEngine;
3 
4 namespace droid.Runtime.Utilities.Misc.Extensions {
8  [RequireComponent(typeof(Joint))]
9  public class JointFix : MonoBehaviour {
10  JointDrive[] _angular_x_drive;
11  Rigidbody[] _connected_bodies;
12  bool[] _enable_processings;
13  float[] _force_break_limits;
14  Vector3 _initial_local_position;
15  Quaternion _initial_local_rotation;
16  Type[] _joint_types;
17 
18  Joint[] _joints;
19  JointLimits[] _limits;
20  SoftJointLimitSpring[] _linear_limit_springs;
21  SoftJointLimit[] _linear_limits;
22  Vector3 _local_position_on_disable;
23 
24  Quaternion _local_rotation_on_disable;
25  JointDrive[] _slerp_drives;
26  Vector3[] _target_angulars;
27  Vector3[] _target_positions;
28  Quaternion[] _target_rotations;
29  Vector3[] _target_velocities;
30  float[] _torque_break_limits;
31 
32  bool _was_disabled;
33  SoftJointLimit[] _x_ang_high_limits;
34  SoftJointLimit[] _x_ang_low_limits;
35  ConfigurableJointMotion[] _x_ang_motion;
36  JointDrive[] _x_drives;
37  ConfigurableJointMotion[] _x_motion;
38  ConfigurableJointMotion[] _y_ang_motion;
39  JointDrive[] _y_drives;
40  ConfigurableJointMotion[] _y_motion;
41  ConfigurableJointMotion[] _z_ang_motion;
42  JointDrive[] _z_drives;
43  ConfigurableJointMotion[] _z_motion;
44 
45  void Awake() { this.Setup(); }
46 
47  void Setup() {
48  this._initial_local_rotation = this.transform.localRotation;
49  this._initial_local_position = this.transform.localPosition;
50  this._joints = this.GetComponents<Joint>();
51  var length = this._joints.Length;
52  this._connected_bodies = new Rigidbody[length];
53  this._joint_types = new Type[length];
54  this._x_ang_low_limits = new SoftJointLimit[length];
55  this._x_ang_high_limits = new SoftJointLimit[length];
56  this._limits = new JointLimits[length];
57  this._force_break_limits = new float[length];
58  this._torque_break_limits = new float[length];
59  this._x_motion = new ConfigurableJointMotion[length];
60  this._y_motion = new ConfigurableJointMotion[length];
61  this._z_motion = new ConfigurableJointMotion[length];
62  this._x_ang_motion = new ConfigurableJointMotion[length];
63  this._y_ang_motion = new ConfigurableJointMotion[length];
64  this._z_ang_motion = new ConfigurableJointMotion[length];
65  this._angular_x_drive = new JointDrive[length];
66  this._target_rotations = new Quaternion[length];
67  this._target_angulars = new Vector3[length];
68  this._target_positions = new Vector3[length];
69  this._target_velocities = new Vector3[length];
70  this._x_drives = new JointDrive[length];
71  this._y_drives = new JointDrive[length];
72  this._z_drives = new JointDrive[length];
73  this._linear_limits = new SoftJointLimit[length];
74  this._linear_limit_springs = new SoftJointLimitSpring[length];
75  this._slerp_drives = new JointDrive[length];
76  this._enable_processings = new bool[length];
77 
78  for (var i = 0; i < length; i++) {
79  this._connected_bodies[i] = this._joints[i].connectedBody;
80  this._joint_types[i] = this._joints[i].GetType();
81  this._force_break_limits[i] = this._joints[i].breakForce;
82  this._torque_break_limits[i] = this._joints[i].breakTorque;
83  this._enable_processings[i] = this._joints[i].enablePreprocessing;
84  if (this._joints[i] is HingeJoint) {
85  this._limits[i] = ((HingeJoint)this._joints[i]).limits;
86  } else if (this._joints[i] is ConfigurableJoint) {
87  this._x_ang_low_limits[i] = ((ConfigurableJoint)this._joints[i]).lowAngularXLimit;
88  this._x_ang_high_limits[i] = ((ConfigurableJoint)this._joints[i]).highAngularXLimit;
89  this._x_motion[i] = ((ConfigurableJoint)this._joints[i]).xMotion;
90  this._y_motion[i] = ((ConfigurableJoint)this._joints[i]).yMotion;
91  this._z_motion[i] = ((ConfigurableJoint)this._joints[i]).zMotion;
92  this._x_ang_motion[i] = ((ConfigurableJoint)this._joints[i]).angularXMotion;
93  this._y_ang_motion[i] = ((ConfigurableJoint)this._joints[i]).angularYMotion;
94  this._z_ang_motion[i] = ((ConfigurableJoint)this._joints[i]).angularZMotion;
95  this._angular_x_drive[i] = ((ConfigurableJoint)this._joints[i]).angularXDrive;
96  this._target_rotations[i] = ((ConfigurableJoint)this._joints[i]).targetRotation;
97  this._linear_limits[i] = ((ConfigurableJoint)this._joints[i]).linearLimit;
98  this._slerp_drives[i] = ((ConfigurableJoint)this._joints[i]).slerpDrive;
99  this._linear_limit_springs[i] = ((ConfigurableJoint)this._joints[i]).linearLimitSpring;
100  this._x_drives[i] = ((ConfigurableJoint)this._joints[i]).xDrive;
101  this._y_drives[i] = ((ConfigurableJoint)this._joints[i]).yDrive;
102  this._z_drives[i] = ((ConfigurableJoint)this._joints[i]).zDrive;
103  this._target_positions[i] = ((ConfigurableJoint)this._joints[i]).targetPosition;
104  this._target_velocities[i] = ((ConfigurableJoint)this._joints[i]).targetVelocity;
105  this._target_angulars[i] = ((ConfigurableJoint)this._joints[i]).targetAngularVelocity;
106  }
107  }
108  }
109 
110  void OnDisable() {
111  this._local_rotation_on_disable = this.transform.localRotation;
112  this.transform.localRotation = this._initial_local_rotation;
113 
114  this._local_position_on_disable = this.transform.localPosition;
115  this.transform.localPosition = this._initial_local_position;
116 
117  this._was_disabled = true;
118  }
119 
120  void Update() {
121  if (this._was_disabled) {
122  this._was_disabled = false;
123  this.transform.localRotation = this._local_rotation_on_disable;
124  this.transform.localPosition = this._local_position_on_disable;
125  }
126  }
127 
130  public void Reset() {
131  if (this._joints == null) {
132  this.Setup();
133  }
134 
135  this.transform.localRotation = this._initial_local_rotation;
136  this.transform.localPosition = this._initial_local_position;
137  for (var i = 0; i < this._joints.Length; i++) {
138  if (this._joints[i] == null) {
139  this._joints[i] = (Joint)this.gameObject.AddComponent(this._joint_types[i]);
140  this._joints[i].connectedBody = this._connected_bodies[i];
141  }
142 
143  this._joints[i].breakForce = this._force_break_limits[i];
144  this._joints[i].breakTorque = this._torque_break_limits[i];
145  this._joints[i].enablePreprocessing = this._enable_processings[i];
146 
147  if (this._joints[i] is HingeJoint) {
148  ((HingeJoint)this._joints[i]).limits = this._limits[i];
149  } else if (this._joints[i] is SpringJoint) {
150  //((SpringJoint)this._joints[i]).anchor = this.anchor[i];
151  } else if (this._joints[i] is ConfigurableJoint) {
152  ((ConfigurableJoint)this._joints[i]).lowAngularXLimit = this._x_ang_low_limits[i];
153  ((ConfigurableJoint)this._joints[i]).highAngularXLimit = this._x_ang_high_limits[i];
154  ((ConfigurableJoint)this._joints[i]).xMotion = this._x_motion[i];
155  ((ConfigurableJoint)this._joints[i]).yMotion = this._y_motion[i];
156  ((ConfigurableJoint)this._joints[i]).zMotion = this._z_motion[i];
157  ((ConfigurableJoint)this._joints[i]).angularXMotion = this._x_ang_motion[i];
158  ((ConfigurableJoint)this._joints[i]).angularYMotion = this._y_ang_motion[i];
159  ((ConfigurableJoint)this._joints[i]).angularZMotion = this._z_ang_motion[i];
160  ((ConfigurableJoint)this._joints[i]).angularXDrive = this._angular_x_drive[i];
161  ((ConfigurableJoint)this._joints[i]).targetRotation = this._target_rotations[i];
162  ((ConfigurableJoint)this._joints[i]).linearLimit = this._linear_limits[i];
163  ((ConfigurableJoint)this._joints[i]).linearLimitSpring = this._linear_limit_springs[i];
164  ((ConfigurableJoint)this._joints[i]).xDrive = this._x_drives[i];
165  ((ConfigurableJoint)this._joints[i]).yDrive = this._y_drives[i];
166  ((ConfigurableJoint)this._joints[i]).zDrive = this._z_drives[i];
167  ((ConfigurableJoint)this._joints[i]).targetPosition = this._target_positions[i];
168  ((ConfigurableJoint)this._joints[i]).targetVelocity = this._target_velocities[i];
169  ((ConfigurableJoint)this._joints[i]).targetAngularVelocity = this._target_angulars[i];
170  ((ConfigurableJoint)this._joints[i]).slerpDrive = this._slerp_drives[i];
171  }
172  }
173  }
174  }
175 }