Ejemplo n.º 1
0
    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
        }
    }
Ejemplo n.º 2
0
    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
                );
        }
    }
Ejemplo n.º 3
0
        // #################################  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;
            }
        }
Ejemplo n.º 4
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);
        }
    }
Ejemplo n.º 5
0
    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;
    }
Ejemplo n.º 6
0
    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;
    }
Ejemplo n.º 7
0
    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));
    }
Ejemplo n.º 8
0
    // ----- ----- ----- ----- -----

    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)
                   ));
    }
Ejemplo n.º 9
0
    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);
    }
Ejemplo n.º 10
0
    public PosRot TransformPosRot(PosRot posrot)
    {
        PosRot result = new PosRot();

        result.position = position + rotation * posrot.position;
        result.rotation = rotation * posrot.rotation;
        return(result);
    }
Ejemplo n.º 11
0
    public PosRot Inverse()
    {
        PosRot result = new PosRot();

        result.rotation = Quaternion.Inverse(rotation);
        result.position = -(result.rotation * position);
        return(result);
    }
Ejemplo n.º 12
0
    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);
    }
Ejemplo n.º 13
0
    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));
    }
Ejemplo n.º 14
0
        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;
            }
        }
Ejemplo n.º 15
0
 /// <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));
     }
 }
Ejemplo n.º 16
0
    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();
    }
Ejemplo n.º 17
0
    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;
    }
Ejemplo n.º 18
0
    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;
        }
    }
Ejemplo n.º 19
0
    /// <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();
            }
        }
    }
Ejemplo n.º 20
0
        //-----------------------------------------------------------------------------------
        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);
        }
Ejemplo n.º 21
0
    //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);
    }
Ejemplo n.º 22
0
    /// <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);
    }
Ejemplo n.º 23
0
 //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);
 }
Ejemplo n.º 24
0
        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);
            }
        }
Ejemplo n.º 25
0
    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);
    }
Ejemplo n.º 26
0
    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);
            }
        }
    }
Ejemplo n.º 27
0
    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);
    }
Ejemplo n.º 28
0
        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");
        }
Ejemplo n.º 29
0
    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);
        }
    }
Ejemplo n.º 30
0
    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;
        }
    }
Ejemplo n.º 31
0
 void RpcChangePos(PosRot pos)
 {
     //Logic.GlassRemote remote = (Logic.GlassRemote)m_glass;
     //remote.SetPos(pos, false);
 }
Ejemplo n.º 32
0
 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);
 }
Ejemplo n.º 33
0
 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);
 }
Ejemplo n.º 34
0
 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);
     }
 }
Ejemplo n.º 35
0
 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;
     }
 }