void AttachHead(Collider new_head)
{
SpringJoint spring = GetComponent <SpringJoint>();
RemoveLine();
if (spring != null)
{
Destroy(spring);
}
Vector3[] faces = { new_head.transform.up, -new_head.transform.up,
new_head.transform.right, -new_head.transform.right,
new_head.transform.forward, -new_head.transform.forward };
float greatest_dot = 0.0f;
Vector3 attach_face = Vector3.zero;
for (int i = 0; i < faces.Length; ++i)
{
float dot = Vector3.Dot(-transform.up, faces[i]);
if (dot > greatest_dot)
{
attach_face = faces[i];
greatest_dot = dot;
}
}
new_head.transform.rotation = Quaternion.FromToRotation(attach_face, -transform.up) *
new_head.transform.rotation;
greatest_dot = 0.0f;
Vector3 robot_face_vector = Vector3.zero;
for (int i = 0; i < faces.Length; ++i)
{
float dot = Vector3.Dot(transform.forward, faces[i]);
if (dot > greatest_dot)
{
robot_face_vector = faces[i];
greatest_dot = dot;
}
}
FixedJoint head_joint = gameObject.AddComponent <FixedJoint>();
head_joint.connectedBody = new_head.GetComponent <Rigidbody>();
head_joint.anchor = headCenterOffset;
head_joint.autoConfigureConnectedAnchor = false;
head_joint.connectedAnchor = Vector3.zero;
head_joint.breakForce = breakForce;
head_joint.breakTorque = breakTorque;
if (robot_head_ != new_head.GetComponent <RobotHead>())
{
robot_head_ = new_head.GetComponent <RobotHead>();
robot_head_.SetBody(this);
robot_head_.SetOrientation(transform.up, robot_face_vector);
robot_head_.BootUp();
}
}
void FixedUpdate()
{
FixedJoint head_joint = GetComponent <FixedJoint>();
if (head_joint == null)
{
LookForHead();
}
if (robot_head_ != null)
{
Vector3[] faces = { robot_head_.transform.up, -robot_head_.transform.up,
robot_head_.transform.right, -robot_head_.transform.right,
robot_head_.transform.forward, -robot_head_.transform.forward };
float greatest_dot = 0.0f;
Vector3 robot_face_vector = Vector3.zero;
for (int i = 0; i < faces.Length; ++i)
{
float dot = Vector3.Dot(transform.forward, faces[i]);
if (dot > greatest_dot)
{
robot_face_vector = faces[i];
greatest_dot = dot;
}
}
robot_head_.SetOrientation(transform.up, robot_face_vector);
}
Quaternion uprightness = Quaternion.FromToRotation(Vector3.up, transform.up);
float upright_angle = 0;
Vector3 upright_axis = Vector3.up;
uprightness.ToAngleAxis(out upright_angle, out upright_axis);
is_upright_ = upright_angle < uprightAngle;
if (!is_upright_)
{
fallen_time_ += Time.deltaTime;
if (upright_angle < upsideDownAngle && fallen_time_ > fallenWaitTime)
{
GetComponent <Rigidbody>().AddTorque(-uprightForce * upright_axis);
}
}
else
{
fallen_time_ = 0.0f;
}
}
void AttachHead(Collider new_head)
{
SpringJoint spring = GetComponent<SpringJoint>();
RemoveLine();
if (spring != null)
Destroy(spring);
Vector3[] faces = {new_head.transform.up, -new_head.transform.up,
new_head.transform.right, -new_head.transform.right,
new_head.transform.forward, -new_head.transform.forward};
float greatest_dot = 0.0f;
Vector3 attach_face = Vector3.zero;
for (int i = 0; i < faces.Length; ++i) {
float dot = Vector3.Dot(-transform.up, faces[i]);
if (dot > greatest_dot) {
attach_face = faces[i];
greatest_dot = dot;
}
}
new_head.transform.rotation = Quaternion.FromToRotation(attach_face, -transform.up) *
new_head.transform.rotation;
greatest_dot = 0.0f;
Vector3 robot_face_vector = Vector3.zero;
for (int i = 0; i < faces.Length; ++i) {
float dot = Vector3.Dot(transform.forward, faces[i]);
if (dot > greatest_dot) {
robot_face_vector = faces[i];
greatest_dot = dot;
}
}
FixedJoint head_joint = gameObject.AddComponent<FixedJoint>();
head_joint.connectedBody = new_head.GetComponent<Rigidbody>();
head_joint.anchor = headCenterOffset;
head_joint.autoConfigureConnectedAnchor = false;
head_joint.connectedAnchor = Vector3.zero;
head_joint.breakForce = breakForce;
head_joint.breakTorque = breakTorque;
if (robot_head_ != new_head.GetComponent<RobotHead>()) {
robot_head_ = new_head.GetComponent<RobotHead>();
robot_head_.SetBody(this);
robot_head_.SetOrientation(transform.up, robot_face_vector);
robot_head_.BootUp();
}
}
