public void Link(RobotLink link)
{
prev = link;
Multiply(new float[4, 4] {
{ 1, 0, 0, 0 }, { 0, 1, 0, 0 }, { 0, 0, 1, 0 }, { -link.t.position.x, -link.t.position.y, -link.t.position.z, 1 }
});
}
private void Rotate(RobotLink part, int deg)
{
float c = Mathf.Cos(deg * Mathf.PI / 180);
float s = Mathf.Sin(deg * Mathf.PI / 180);
part.Multiply(new float[4, 4] {
{ 1, 0, 0, 0 }, { 0, c, s, 0 }, { 0, -s, c, 0 }, { 0, 0, 0, 1 }
});
if (part == arm1)
{
Text text = (Text)GameObject.Find("TextArm1").GetComponent(typeof(Text));
int d = int.Parse(text.text.Substring(0, text.text.Length - 1));
d = (d + deg + 360) % 360;
d = d > 180 ? d - 360 : d;
text.text = d + "°";
}
else if (part == arm2)
{
Text text = (Text)GameObject.Find("TextArm2").GetComponent(typeof(Text));
int d = int.Parse(text.text.Substring(0, text.text.Length - 1));
d = (d + deg + 360) % 360;
d = d > 180 ? d - 360 : d;
text.text = d + "°";
}
else if (part == arm3)
{
Text text = (Text)GameObject.Find("TextArm3").GetComponent(typeof(Text));
int d = int.Parse(text.text.Substring(0, text.text.Length - 1));
d = (d + deg + 360) % 360;
d = d > 180 ? d - 360 : d;
text.text = d + "°";
}
}
void Start()
{
Transform t = transform.Find("Static Base");
staticbase = new RobotLink(t, t.position.x, t.position.y, t.position.z);
t = transform.Find("Dynamic Base");
dynamicbase = new RobotLink(t, t.position.x - staticbase.t.position.x, t.position.y - staticbase.t.position.y, t.position.z - staticbase.t.position.z);
t = transform.Find("Arm1");
arm1 = new RobotLink(t, t.position.x - dynamicbase.t.position.x, t.position.y - dynamicbase.t.position.y, t.position.z - dynamicbase.t.position.z);
t = transform.Find("Arm2");
arm2 = new RobotLink(t, t.position.x - arm1.t.position.x, t.position.y - arm1.t.position.y, t.position.z - arm1.t.position.z);
t = transform.Find("Arm3");
arm3 = new RobotLink(t, t.position.x - arm2.t.position.x, t.position.y - arm2.t.position.y, t.position.z - arm2.t.position.z);
arm3.prev = arm2;
arm2.prev = arm1;
arm1.prev = dynamicbase;
dynamicbase.prev = staticbase;
// SIMPLE ROTATION ABOUT THE X AXIS BY X DEGREES
//float x = 30;
//float c = Mathf.Cos (x * Mathf.PI / 180);
//float s = Mathf.Sin (x * Mathf.PI / 180);
//arm1.Multiply (new float[4,4] {{1,0,0,0},{0,c,s,0},{0,-s,c,0},{0,0,0,1}});
circleSprites = Resources.LoadAll <Sprite>("blue_circle");
}
public void StopRotate()
{
if (client != null)
{
client.StopRotate(BitConverter.GetBytes(DateTime.UtcNow.Ticks));
}
part = null;
}
public void RotatePointerR()
{
if (client != null)
{
client.RotatePointerR(BitConverter.GetBytes(DateTime.UtcNow.Ticks));
}
part = pointer;
deg = 1;
}
public void RotateArm2R()
{
part = arm2;
deg = -1;
if (client != null)
{
client.RotateArm2R(BitConverter.GetBytes(DateTime.UtcNow.Ticks));
}
}
public void RotateArm1R()
{
if (client != null)
{
client.RotateArm1R(BitConverter.GetBytes(DateTime.UtcNow.Ticks));
}
part = arm1;
deg = 1;
}
/// <summary>
/// SpawnJoint: Spawns a new joint object.
/// </summary>
public static GameObject SpawnLink()
{
Vector3 SpawnPosition = new Vector3(0.05f, 0f, -5.8f);
GameObject newLink = GameObject.CreatePrimitive(PrimitiveType.Capsule);
newLink.transform.position = SpawnPosition;
RobotLink LinkComponent = newLink.AddComponent <RobotLink>();
return(newLink);
}
/// <summary>
/// Helps ConfigureRobot by adding into the dict and list, recurses onto the next child
/// </summary>
private static void addToList(RobotJoint[] listJoints, RobotLink theTree, string RobotType, List <RobotJoint> originalList = null)
{
foreach (RobotJoint joint in listJoints)
{
theTree.JointDict.Add(joint, theTree);
originalList.Add(joint);
if (joint.ChildGO != null)
{
ConfigureRobot(joint.ChildGO.gameObject, RobotType, false, theTree, originalList);
}
}
}
/// <summary>
/// Initializer
/// </summary>
public RobotLink(GameObject sObject, string typeBase, bool iRoot, RobotLink parent = null)
{
Rigidbody thisObjectRB = sObject.GetComponent <Rigidbody>();
SelfObject = sObject;
BaseType = typeBase;
isRoot = iRoot;
Parent = parent;
localCOM = thisObjectRB.centerOfMass;
inertiaTensor = thisObjectRB.inertiaTensor;
mass = thisObjectRB.mass;
// If it's a root and floating, then we need to store the position as well as the rotation
if (isRoot && BaseType.ToLower() == "floating")
{
RootPosition = thisObjectRB.position;
RootRotation = thisObjectRB.rotation;
}
}
/// <summary>
/// GenerateJoint: Helper method to recreate links by:
/// 1. Creating the appropriate GameObject
/// 2. Setting its position properties.
/// 3. Setting misc. properties as well as removing the collider.
///
/// TODO: Need to set COM and inertiatensors upon initialization.
/// TODO: See if link component is actually useful.
///
/// <param name="linkConfig"> A protobuf object holding all of the necessary information to recreate a link. </param>
///
/// <returns> A GameObject representing a link of a robot. </returns>
/// </summary>
public static GameObject GenerateLink(LinkStorage linkConfig)
{
GameObject newLink = GenerateShape(linkConfig.Shape);
newLink.transform.localScale = new Vector3(linkConfig.Shape.ScaleParams[0], linkConfig.Shape.ScaleParams[1], linkConfig.Shape.ScaleParams[2]);
newLink.transform.position = new Vector3(linkConfig.PositionParams[0], linkConfig.PositionParams[1], linkConfig.PositionParams[2]);
newLink.transform.rotation = new Quaternion(linkConfig.RotationParams[0], linkConfig.RotationParams[1], linkConfig.RotationParams[2], linkConfig.RotationParams[3]);
// There should be code here to set COM and inertiatensors.
RobotLink newRobotLink = newLink.AddComponent <RobotLink>();
Rigidbody newRigid = newLink.AddComponent <Rigidbody>();
newRigid.isKinematic = true;
newRigid.useGravity = false;
Destroy(newLink.GetComponent <Collider>());
return(newLink);
}
void Start()
{
staticbase = new RobotLink(transform.Find("StaticBase"));
dynamicbase = new RobotLink(transform.Find("DynamicBase"));
arm1 = new RobotLink(transform.Find("Arm1"));
arm2 = new RobotLink(transform.Find("Arm2"));
pointer = new RobotLink(transform.Find("Pointer"));
pointer.Link(arm2);
arm2.Link(arm1);
arm1.Link(dynamicbase);
dynamicbase.Link(staticbase);
// SIMPLE ROTATION ABOUT THE X AXIS BY X DEGREES
//float x = 30;
//float c = Mathf.Cos (x * Mathf.PI / 180);
//float s = Mathf.Sin (x * Mathf.PI / 180);
//arm1.Multiply (new float[4,4] {{1,0,0,0},{0,c,s,0},{0,-s,c,0},{0,0,0,1}});
circleSprites = Resources.LoadAll <Sprite>("blue_circle");
}
/// <summary>
/// Makes a new "RobotLink" instance given the root of a robot
/// </summary>
public static RobotLink ConfigureRobot(GameObject rootObject, string RobotType, bool _isTheRoot = true, RobotLink parentTree = null, List <RobotJoint> originalList = null)
{
RobotJoint[] robotJoints = rootObject.GetComponents <RobotJoint>();
if (_isTheRoot)
{
RobotLink newRobotLink = new RobotLink(rootObject, RobotType, true);
newRobotLink.BaseType = RobotType;
addToList(robotJoints, newRobotLink, RobotType, newRobotLink.RobotJoints);
return(newRobotLink);
}
else
{
RobotLink newRobotLink = new RobotLink(rootObject, RobotType, false);
newRobotLink.BaseType = RobotType;
newRobotLink.Parent = parentTree;
addToList(robotJoints, newRobotLink, RobotType, originalList);
return(null);
}
}
/// <summary>
/// Method to recreate a link
/// </summary>
public static GameObject GenerateLink(LinkStorage linkConfig)
{
GameObject newLink = GenerateShape(linkConfig.Shape);
newLink.transform.localScale = new Vector3(linkConfig.Shape.ScaleParams[0], linkConfig.Shape.ScaleParams[1], linkConfig.Shape.ScaleParams[2]);
newLink.transform.position = new Vector3(linkConfig.PositionParams[0], linkConfig.PositionParams[1], linkConfig.PositionParams[2]);
newLink.transform.rotation = new Quaternion(linkConfig.RotationParams[0], linkConfig.RotationParams[1], linkConfig.RotationParams[2], linkConfig.RotationParams[3]);
// There should be code here to set COM and inertiatensors, but will leave that for later.
// Also will do more stuff with this link component... For now it's just... uh... kind of useless
RobotLink newRobotLink = newLink.AddComponent <RobotLink>();
Rigidbody newRigid = newLink.AddComponent <Rigidbody>();
newRigid.isKinematic = true;
newRigid.useGravity = false;
Destroy(newLink.GetComponent <Collider>());
return(newLink);
}
void Update()
{
if (_material != null)
{
// The target color for the Interaction object will be determined by various simple state checks.
Color targetColor = defaultColor;
if (_intObj.isHovered && useHover)
{
float glow = _intObj.closestHoveringControllerDistance.Map(0F, 0.2F, 1F, 0.0F);
targetColor = Color.Lerp(defaultColor, hoverColor, glow);
}
// On press
if (_intObj is InteractionButton && (_intObj as InteractionButton).isPressed)
{
if (!_isPushed)
{
_isPushed = true;
switch (ButtonChoice)
{
case ButtonType.SpawnLink:
GameObject newLink = RobotLink.SpawnLink();
GameObject Linktool = SpawnTransformTool(newLink);
LinkIDCount += 1;
newLink.GetComponent <RobotLink>().SelfID = LinkIDCount;
newLink.transform.localScale = spawnScale;
newLink.transform.position = spawnLocation;
break;
case ButtonType.SpawnJoint:
GameObject newJoint = ObjectJoint.SpawnJoint();
GameObject Jointtool = SpawnTransformTool(newJoint);
JointIDCount += 1;
newJoint.GetComponent <ObjectJoint>().SelfID = JointIDCount;
newJoint.transform.localScale = spawnScale;
newJoint.transform.position = spawnLocation;
break;
case ButtonType.Attach:
AttachObjects();
break;
case ButtonType.Delete:
DeleteObject();
break;
}
targetColor = pressedColor;
}
}
// For the button cooldown
if (_intObj is InteractionButton && !(_intObj as InteractionButton).isPressed)
{
_isPushed = false;
}
// Lerp actual material color to the target color.
_material.color = Color.Lerp(_material.color, targetColor, 30F * Time.deltaTime);
}
}
private void SpawnLink()
{
GameObject newLink = RobotLink.SpawnLink();
newLink.AddComponent <ClickerTest>();
}
void StopRotate()
{
part = null;
}
void RotateArm3R()
{
part = arm3;
deg = 1;
}
void RotateArm3L()
{
part = arm3;
deg = -1;
}
void RotateArm2R()
{
part = arm2;
deg = -1;
}
void RotateArm2L()
{
part = arm2;
deg = 1;
}
void RotateArm1R()
{
part = arm1;
deg = 1;
}
void RotateArm1L()
{
part = arm1;
deg = -1;
}
void RotatePointerR()
{
part = pointer;
deg = 1;
}
void RotatePointerL()
{
part = pointer;
deg = -1;
}
