private void Update()
{
// Move our position a step closer to the target.
float step = speed * Time.deltaTime; // calculate distance to move
cube.transform.position = Vector3.MoveTowards(cube.transform.position, destination, step);
if (Vector3.Distance(cube.transform.position, destination) < delta && Time.time > awaitingResponseUntilTimestamp)
{
Debug.Log("Destination reached.");
PosRot cubePos = new PosRot(
cube.transform.position.x,
cube.transform.position.y,
cube.transform.position.z,
cube.transform.rotation.x,
cube.transform.rotation.y,
cube.transform.rotation.z,
cube.transform.rotation.w
);
PositionServiceRequest positionServiceRequest = new PositionServiceRequest(cubePos);
// Send message to ROS and return the response
ros.SendServiceMessage <PositionServiceResponse>(serviceName, positionServiceRequest, Callback_Destination);
awaitingResponseUntilTimestamp = Time.time + 1.0f; // don't send again for 1 second, or until we receive a response
}
}
public void Action()
{
PosRot moveTo = new PosRot(gameObject);
PosRot autoReturnTo = new PosRot(reachController.trajectory.Last().p1, reachController.trajectory.Last().q1);
// ----- ----- -----
PosRot relativeKeyPosRot = new PosRot(lookController.body["Base"].transform).Inverse().TransformPosRot(moveTo);
Vector3 originPos = lookController.body["Base"].transform.position;
Quaternion originRot = Quaternion.Slerp(lookController.body["Base"].transform.rotation, lookController.currentTargetPose.rotation, relativeMoveRatio);
moveTo = new PosRot(originPos, originRot).TransformPosRot(relativeKeyPosRot);
// ----- ----- -----
reachController.AddSubMovement(
moveTo,
springDamper,
duration,
duration
);
// ----- ----- -----
if (autoReturn && returnDuration > 1e-5 && returnStartTime > 1e-5)
{
reachController.AddSubMovement(
autoReturnTo,
springDamper,
returnStartTime + returnDuration,
returnDuration
);
}
}
// ################################# Getters ############################################
// void getPosOri(ArticulationBody link)
// {
// Transform data = link.transform;
// // //print(data.eulerAngles);
// // print(data.position);
// return
// }
// void getEEinfo()
// {
// }
// ################################# Publishers #################################
void publishStates()
{
timeElapsed += Time.deltaTime;
if (timeElapsed > publishMessageFrequency)
{
Transform grasp_target = articulationChain[ee_index].transform;
Transform ee_info = articulationChain[ee_index - 1].transform;
PosRot eePosRot = new PosRot(
grasp_target.position.x,
grasp_target.position.y,
grasp_target.position.z,
ee_info.rotation.x,
ee_info.rotation.y,
ee_info.rotation.z,
ee_info.rotation.w
);
print(ee_info.rotation);
print(ee_info.eulerAngles);
print(grasp_target.position);
// Finally send the message to server_endpoint.py running in ROS
ros.Send(topicName, eePosRot);
timeElapsed = 0;
}
}
public void LaunchRocket(Whose atWhose, Position targetPosition)
{
// gav4.LogEvent (CATEGORY, "LaunchRocket", atWhose.ToString (), 0);
RocketController rocketController = GetGridController(atWhose).MakeRocket();
Vector3 localTargetPos = targetPosition.AsGridLocalPosition(Marker.Aim);
PosRot start = new PosRot(rocketController.transform);
Transform targetGridTransform = GetGridController(atWhose).transform;
Vector3 pos = targetGridTransform.position + (targetGridTransform.rotation * localTargetPos);
pos += targetGridTransform.right * .5f + targetGridTransform.up * .5f;
PosRot end = new PosRot(pos, targetGridTransform.rotation);
firing++;
rocketFlightTime = Mathf.Max(1f, .95f * rocketFlightTime);
rocketController.Launch(atWhose, targetPosition, start, end, rocketFlightTime, delegate {
firing--;
});
rocketsLaunched++;
if (atWhose == Whose.Ours && rocketsLaunched < 7)
{
float duration = .7f * rocketFlightTime;
float delay = rocketFlightTime - duration;
centerPanelController.IssueWarning(delay, duration);
}
}
public void Launch(Whose atWhose, Position targetPosition, PosRot start, PosRot end, float flightTime, Action callback)
{
this.callback = callback;
this.atWhose = atWhose;
this.targetPosition = targetPosition;
this.rocketFlightTime = flightTime;
t0 = Time.time;
float distance = Vector3.Distance(start.position, end.position);
bezier.t0.position = start.position;
bezier.t0.rotation = start.rotation;
bezier.t3.position = end.position;
bezier.t3.rotation = end.rotation;
bezier.t1.position = start.position + .3f * distance * bezier.t0.forward + Deviation(bezier.t0);
bezier.t1.rotation = start.rotation;
bezier.t2.position = end.position - .3f * distance * bezier.t3.forward;
bezier.t2.rotation = end.rotation;
transform.position = start.position;
transform.rotation = start.rotation;
}
public void LookAtTransform(Transform transf, Vector3 source, float dist, float hRot, float vRot)
{
PosRot look = LookAt(source, dist, hRot, vRot);
transf.position = look.position;
transf.rotation = look.rotation;
}
public static PosRot Interpolate(PosRot px0y0, PosRot px1y0, PosRot px0y1, PosRot px1y1, float x, float y)
{
PosRot py0 = Interpolate(px0y0, px1y0, x);
PosRot py1 = Interpolate(px0y1, px1y1, x);
return(Interpolate(py0, py1, y));
}
// ----- ----- ----- ----- -----
public static PosRot Interpolate(PosRot px0, PosRot px1, float x)
{
return(new PosRot(
px1.position * x + px0.position * (1 - x),
Quaternion.Slerp(px0.rotation, px1.rotation, x)
));
}
public static PosRot Rotate(PosRot px0, Quaternion rotation, Vector3 center)
{
PosRot px1 = new PosRot();
px1.position = rotation * (px0.position - center) + center;
px1.rotation = rotation * px0.rotation;
return(px1);
}
public PosRot TransformPosRot(PosRot posrot)
{
PosRot result = new PosRot();
result.position = position + rotation * posrot.position;
result.rotation = rotation * posrot.rotation;
return(result);
}
public PosRot Inverse()
{
PosRot result = new PosRot();
result.rotation = Quaternion.Inverse(rotation);
result.position = -(result.rotation * position);
return(result);
}
public static PosRot Parse(string line)
{
var data = line.Split(',').Select(s => float.Parse(s));
PosRot pose = new PosRot();
pose.position = new Vector3(data.ElementAt(0), data.ElementAt(1), data.ElementAt(2));
pose.rotation = new Quaternion(data.ElementAt(3), data.ElementAt(4), data.ElementAt(5), data.ElementAt(6));
return(pose);
}
private bool Inside(Vector3 point, GeoRegion region)
{
PosRot center = region.transform.posRot;
bool box = region.shape == GeoRegion.GeoShape.box;
float radius = region.sphereRadius + GEOM_DELTA;
Vector3 extents = region.extents + 2f * GEOM_DELTA * Vector3.one;
return(UFUtils.Inside(point, center, radius, extents, box));
}
void OnCollisionStay(Collision collisionInfo)
{
//if (collisionInfo.rigidbody != part.Rigidbody || !moduleAnimateGeneric)
if (!moduleAnimateGeneric || !animationStopper)
{
return;
}
if (!moduleAnimateGeneric.IsMoving())
{
return;
}
foreach (ContactPoint contact in collisionInfo.contacts)
{
ColliderInfo thisColliderInfo = GetColliderInfo(contact.thisCollider);
// Check if this is an internal collider moving in respect to the part,
// or if it is a child part moving due to being animated by AnimatedAttachment
if (thisColliderInfo.part == part)
{
PosRot currentPosRot = PosRot.GetPosRot(thisColliderInfo.collider.transform, part);
Vector3 movement = currentPosRot.position - thisColliderInfo.posRot.position;
double angle = Quaternion.Angle(currentPosRot.rotation, thisColliderInfo.posRot.rotation);
thisColliderInfo.posRot = currentPosRot;
// Set a minimum level
if (movement.IsSmallerThan(0.1f) && angle < 0.1)
{
continue;
}
printf("Animation stopped by a collision between %s and %s (movement %s, angle %s)",
thisColliderInfo.collider.name,
collisionInfo.collider.name,
movement,
angle);
}
else
{
printf("Animation stopped by a collision between %s.%s and %s",
thisColliderInfo.part.name,
thisColliderInfo.collider.name,
collisionInfo.collider.name);
}
float time = moduleAnimateGeneric.animTime;
if (moduleAnimateGeneric.revClampPercent)
{
time = 1 - time;
}
moduleAnimateGeneric.deployPercent = 100.0f * time;
moduleAnimateGeneric.allowDeployLimit = true;
break;
}
}
/// <summary>
/// Returns true if the given point lies in a box or sphere with the given parameters.
/// </summary>
public static bool Inside(Vector3 point, PosRot center, float radius, Vector3 extents, bool box)
{
if (box)
{
return(InsideBox(point, new CenteredBox(center, extents)));
}
else
{
return(InsideSphere(point, center.position, radius));
}
}
public SavedData(int size, int cubeCount)
{
cubeSize = size;
posRot = new PosRot[cubeCount];
for (uint i = 0u; i < posRot.Length; ++i)
{
posRot[i] = new PosRot();
}
rubiksPosRot = new PosRot();
}
public static void DrawPosRot(PosRot pr)
{
Color save = Gizmos.color;
Gizmos.color = Color.blue;
Gizmos.DrawRay(pr.pos, pr.rot * Vector3.forward * 0.5f);
Gizmos.color = Color.red;
Gizmos.DrawRay(pr.pos, pr.rot * Vector3.right * 0.333f);
Gizmos.color = Color.green;
Gizmos.DrawRay(pr.pos, pr.rot * Vector3.up * 0.333f);
Gizmos.color = save;
}
protected void RecieveNetwork(Vector3 pos, Quaternion rot, uLink.NetworkMessageInfo info)
{
PosRot posRot = new PosRot();
if (this.hasInterp && this._interp)
{
posRot.position = pos;
posRot.rotation = rot;
this.rigidbody.isKinematic = true;
this._interp.SetGoals(posRot, info.timestamp);
this._interp.running = true;
}
}
/// <summary>
/// Update movement as pathing from one keyframe to another
/// </summary>
private void PathUpdate()
{
//get movement parameters
int nextKey = GetNextKey();
PosRot fro = keys[lastKey].transform.posRot;
PosRot to = keys[nextKey].transform.posRot;
float distance = (to.position - fro.position).magnitude;
float travTime = GetNextTravTime();
//calculate movement
float x;
if (useTravTimeAsSpd)
{
float speed = travTime;
x = GetXBySpeed(time, speed, distance, lastKey, nextKey, out travTime);
}
else
{
x = GetXByTravTime(time, travTime, distance, lastKey, nextKey);
}
//apply position and rotation
rb.position = Vector3.MoveTowards(fro.position, to.position, x);
ForceOrient(to.position - fro.position);
if (time > travTime)
{
//keyframe is finished, wrap up
TriggerKeyLink(keys[nextKey]);
lastKey = nextKey;
rb.position = to.position;
time -= travTime;
if (travTime < MIN_TRAV_TIME)
{
time = 0f;
}
if (AtLastKeyInSequence())
{
FinishSequence();
}
paused = moving && keys[lastKey].pauseTime > 0f;
if (moving && !paused)
{
PathUpdate();
}
}
}
//-----------------------------------------------------------------------------------
PosRot GetPos()
{
PosRot res = new PosRot();
if (m_glass.figure == null)
{
return(res);
}
res.x = m_glass.figure.pos.x;
res.y = m_glass.figure.pos.y;
res.r = m_glass.figure.rot;
return(res);
}
//public Transform soldier;
void Start()
{
//transforms = GetComponentsInChildren<Transform>();
foreach (Transform t in GetComponentsInChildren<Transform>()){
if(t != transform){
transforms.Add (t);
var pr = new PosRot(t.localPosition,t.localRotation);
parts.Add(pr);
}
}
//InvokeRepeating("Pose",2,3);
//print ("1 "+transforms.Count);
}
/// <summary>
/// Returns travel time to be used on next section of the path.
/// Also returns travel time of the next section when using "useTravTimeAsSpd".
/// the time parameter should always be constrained between 0 and this value.
/// </summary>
private float GetRealTravTime()
{
float travTime = GetNextTravTime();
if (!rotateInPlace && useTravTimeAsSpd)
{
float speed = travTime;
int nextKey = GetNextKey();
PosRot fro = keys[lastKey].transform.posRot;
PosRot to = keys[nextKey].transform.posRot;
float distance = (to.position - fro.position).magnitude;
GetXBySpeed(time, speed, distance, lastKey, nextKey, out travTime);
}
return(travTime);
}
//public Transform soldier;
void Start()
{
//transforms = GetComponentsInChildren<Transform>();
foreach (Transform t in GetComponentsInChildren <Transform>())
{
if (t != transform)
{
transforms.Add(t);
var pr = new PosRot(t.localPosition, t.localRotation);
parts.Add(pr);
}
}
//InvokeRepeating("Pose",2,3);
//print ("1 "+transforms.Count);
}
private void AddColliders(List <ColliderInfo> colliderInfos, Part part)
{
printf("Adding part %s to collider list", part.name);
foreach (Collider collider in part.GetPartColliders())
{
ColliderInfo colliderInfo = new ColliderInfo();
colliderInfo.collider = collider;
colliderInfo.part = part;
colliderInfo.posRot = PosRot.GetPosRot(collider.transform, part);
colliderInfos.Add(colliderInfo);
}
foreach (Part child in part.children)
{
AddColliders(colliderInfos, child);
}
}
PosRot[] GenVertebrae()
{
Quaternion rot =
Quaternion.LookRotation(end - start, Vector3.up);
float len = Vector3.Distance(start, end);
int segmentC = Mathf.CeilToInt(len * ribDensity);
int vertebraC = segmentC + 1;
PosRot[] vertebrae = new PosRot[vertebraC];
for (int i = 0; i < vertebraC; i++)
{
vertebrae[i] = new PosRot(
Vector3.Lerp(start, end, (float)i / segmentC),
rot);
}
return(vertebrae);
}
void Start()
{
playerController = GetComponentInParent <VehicleManager>();
this.arrow = GameObject.Find("Arrow");
this.baseZscale = arrow.transform.localScale.x;
lastCheckpoint = new PosRot(arrow.transform.position, arrow.transform.rotation);
GameObject checkpoint = GameObject.Find("CheckPoints");
if (checkpoint != null)
{
foreach (Transform g in checkpoint.transform)
{
checkpoints.Add(g);
}
}
}
PosRot LookAt(Vector3 source, float dist, float hRot, float vRot)
{
Vector3 norm = new Vector3(0f, 0f, 1f);
norm = RotAround(vRot, norm, new Vector3(1f, 0f, 0f));
norm = RotAround(hRot, norm, new Vector3(0f, 1f, 0f));
norm = norm.normalized;
Vector3 finalPos = source - dist * norm;
Quaternion finalRot = Quaternion.Euler(-vRot, -hRot, 0f);
PosRot pr = new PosRot
{
position = finalPos,
rotation = finalRot
};
return(pr);
}
internal void Load(int index, ConfigNode root)
{
ConfigNode attachNodeInfo = root.GetNode("ATTACH_NODE_INFO", index);
if (attachNodeInfo == null)
{
printf("Failed to find ATTACH_NODE_INFO!");
return;
}
loaded = true;
if (!attachNodeInfo.HasNode("POS_ROT"))
{
printf("Failed to find POS_ROT!");
return;
}
//string attachedPartName = attachNodeInfo.GetValue("attachedPart");
string colliderName = attachNodeInfo.GetValue("colliderName");
Collider[] colliders = animatedAttachment.part.GetPartColliders();
foreach (Collider collider in colliders)
{
if (collider.name == colliderName)
{
this.collider = collider;
}
}
nodeType = (AttachNode.NodeType)Enum.Parse(typeof(AttachNode.NodeType), attachNodeInfo.GetValue("nodeType"));
if (attachedPartOffset == null)
{
attachedPartOffset = new PosRot();
}
attachedPartOffset.Load(attachNodeInfo, "offset");
}
protected void uLink_OnNetworkInstantiate(uLink.NetworkMessageInfo info)
{
PosRot posRot = new PosRot();
this.view = (Facepunch.NetworkView)info.networkView;
uLink.BitStream bitStream = this.view.initialData;
if (this.expectsInitialVelocity)
{
this.initialVelocity = bitStream.ReadVector3();
}
if (this.expectsOwner)
{
this.ownerViewID = bitStream.ReadNetworkViewID();
}
this.spawnTime = info.timestamp;
this.updateInterval = 1 / ((double)NetCull.sendRate * (double)Mathf.Max(1f, this.updateRate));
this.hasInterp = this._interp;
if (this.hasInterp)
{
this._interp.running = false;
}
this.rigidbody.isKinematic = true;
this.__hiding = this.spawnTime > Interpolation.time;
if (!this.__hiding)
{
this.OnShow();
}
else
{
this.OnHide();
if (this.hasInterp)
{
posRot.position = this.view.position;
posRot.rotation = this.view.rotation;
this._interp.SetGoals(posRot, this.spawnTime);
}
InterpTimedEvent.Queue(this, "_init", ref info);
}
}
private void Update()
{
timeElapsed += Time.deltaTime;
if (timeElapsed > publishMessageFrequency)
{
cube.transform.rotation = Random.rotation;
PosRot cubePos = new PosRot(
cube.transform.position.x,
cube.transform.position.y,
cube.transform.position.z,
cube.transform.rotation.x,
cube.transform.rotation.y,
cube.transform.rotation.z,
cube.transform.rotation.w
);
// Finally send the message to server_endpoint.py running in ROS
ros.Send(topicName, cubePos);
timeElapsed = 0;
}
}
void RpcChangePos(PosRot pos)
{
//Logic.GlassRemote remote = (Logic.GlassRemote)m_glass;
//remote.SetPos(pos, false);
}
public static void Lerp(ref PosRot a, ref PosRot b, float t, out PosRot v)
{
v = new PosRot();
v.position = Vector3.Lerp(a.position, b.position, t);
v.rotation = Quaternion.Slerp(a.rotation, b.rotation, t);
}
public static void Lerp(ref PosRot a, ref PosRot b, double t, out PosRot v)
{
PosRot.Lerp(ref a, ref b, (float)t, out v);
}
protected void uLink_OnNetworkInstantiate(uLink.NetworkMessageInfo info)
{
PosRot posRot = new PosRot();
this.view = (Facepunch.NetworkView)info.networkView;
uLink.BitStream bitStream = this.view.initialData;
if (this.expectsInitialVelocity)
{
this.initialVelocity = bitStream.ReadVector3();
}
if (this.expectsOwner)
{
this.ownerViewID = bitStream.ReadNetworkViewID();
}
this.spawnTime = info.timestamp;
this.updateInterval = 1 / ((double)NetCull.sendRate * (double)Mathf.Max(1f, this.updateRate));
this.hasInterp = this._interp;
if (this.hasInterp)
{
this._interp.running = false;
}
this.rigidbody.isKinematic = true;
this.__hiding = this.spawnTime > Interpolation.time;
if (!this.__hiding)
{
this.OnShow();
}
else
{
this.OnHide();
if (this.hasInterp)
{
posRot.position = this.view.position;
posRot.rotation = this.view.rotation;
this._interp.SetGoals(posRot, this.spawnTime);
}
InterpTimedEvent.Queue(this, "_init", ref info);
}
}
protected void RecieveNetwork(Vector3 pos, Quaternion rot, uLink.NetworkMessageInfo info)
{
PosRot posRot = new PosRot();
if (this.hasInterp && this._interp)
{
posRot.position = pos;
posRot.rotation = rot;
this.rigidbody.isKinematic = true;
this._interp.SetGoals(posRot, info.timestamp);
this._interp.running = true;
}
}
