Search Unity

  1. We are migrating the Unity Forums to Unity Discussions. On July 12, the Unity Forums will become read-only.

    Please, do not make any changes to your username or email addresses at id.unity.com during this transition time.

    It's still possible to reply to existing private message conversations during the migration, but any new replies you post will be missing after the main migration is complete. We'll do our best to migrate these messages in a follow-up step.

    On July 15, Unity Discussions will become read-only until July 18, when the new design and the migrated forum contents will go live.


    Read our full announcement for more information and let us know if you have any questions.

Question Script to calculate the position and rotation of a semi trailer based on the path of it's tractor

Discussion in 'Scripting' started by w_adams, Jun 6, 2023.

  1. w_adams

    w_adams

    Joined:
    Apr 7, 2016
    Posts:
    34
    Hi, I have been wrangling this problem for several days, and with several different approaches. The latest approach I am using seems the most promising, but I am having some issues with it I hope to sort out. It's based on an approach I found in a whitepaper here. The pertinent section, 3.4.1, would be a formatting nightmare to post here, so I copied the section as images.





    So, I wrote a script which I believe follows these equations. It uses a spline to describe the path of the tractor. Here is the script:

    Code (CSharp):
    1.  
    2.  
    3. using System.Collections;
    4. using System.Collections.Generic;
    5. using UnityEngine;
    6. using UnityEngine.Splines;
    7.  
    8. public class TrailerMovement : MonoBehaviour
    9. {
    10.     public float d1 = 6.2f;
    11.     public float d2 = .6f;
    12.     public float d3 = 33.4f;
    13.     public float deltaTime = 0.5f;
    14.  
    15.     private float time = 0f;
    16.     private float angleB = 0f;
    17.     private float angleD = 0f;
    18.  
    19.     public SplineContainer testSplineContainer;
    20.     GameObject visualizationObjectParent;
    21.  
    22.     public float simulationDuration = 10f;
    23.  
    24.     private void Update()
    25.     {
    26.         if (Input.GetKeyDown(KeyCode.Space))
    27.         {
    28.             StartCoroutine(runTrailerSim());
    29.         }
    30.     }
    31.  
    32.     private IEnumerator runTrailerSim()
    33.     {
    34.         for (float i = deltaTime; i <= simulationDuration; i+=deltaTime)
    35.         {
    36.             visualizationObjectParent = new GameObject();
    37.             createVisual();
    38.             UpdateTrailerPosition();
    39.             yield return null;
    40.             //yield return new WaitForSeconds(deltaTime);
    41.         }
    42.     }
    43.     void UpdateTrailerPosition()
    44.     {
    45.         // Calculate position and direction of the rear-wheel center point B
    46.         Vector3 positionB = GetPositionB(time);
    47.         Vector3 directionB = GetDirectionB(time);
    48.  
    49.         // Calculate position of the front wheel A
    50.         Vector3 positionA = positionB + (d1 + d2) * new Vector3(Mathf.Sin(angleB), 0f, Mathf.Cos(angleB));
    51.  
    52.         // Calculate angle between tractor and trailer direction
    53.         float anglePsi = angleD - angleB;
    54.  
    55.         // Calculate angular velocity at the center point D of the trailer
    56.         float angularVelocityD = Mathf.Abs(GetVelocityB(time)) / d3 * Mathf.Sin(anglePsi) + Mathf.Abs(GetAngularVelocityB(time)) * d2 / d3 * Mathf.Cos(anglePsi);
    57.  
    58.  
    59.         // Update angles and time
    60.         angleD -= angularVelocityD * deltaTime;
    61.         angleB = GetDirectionB(time + deltaTime).x; // Assuming x-axis represents the direction
    62.  
    63.         // Calculate position of the trailer wheel's rear center point D
    64.         Vector3 positionD = positionB + d2 * new Vector3(Mathf.Sin(angleB), 0f, Mathf.Cos(angleB)) + d3 * new Vector3(-Mathf.Sin(angleD), 0f, -Mathf.Cos(angleD));
    65.         // Update time
    66.         time += deltaTime;
    67.  
    68.         // Use the calculated position and rotation for your trailer
    69.         visualizationObjectParent.transform.rotation = Quaternion.Euler(0f, angleD * Mathf.Rad2Deg, 0f);
    70.         visualizationObjectParent.transform.position = positionD;
    71.  
    72.         //transform.position = positionD;
    73.         //transform.rotation = Quaternion.Euler(0f, angleD * Mathf.Rad2Deg, 0f); // Assuming rotation is around the y-axis
    74.  
    75.     }
    76.  
    77.     Vector3 GetPositionB(float time)
    78.     {
    79.         Vector3 position = testSplineContainer.Spline.EvaluatePosition(time / simulationDuration);
    80.         visualizationObjectParent.transform.position = position;
    81.         return position;
    82.     }
    83.  
    84.     Vector3 GetDirectionB(float time)
    85.     {
    86.         Vector3 tangent = testSplineContainer.Spline.EvaluateTangent(time / simulationDuration);
    87.         tangent.Normalize();
    88.         return tangent;
    89.  
    90.     }
    91.  
    92.     float GetVelocityB(float time)
    93.     {
    94.         Vector3 velocity = (GetPositionB(time) - GetPositionB(time - deltaTime)) / deltaTime;
    95.         return velocity.magnitude;
    96.     }
    97.  
    98.     float GetAngularVelocityB(float time)
    99.     {
    100.         float directionAtTime = CalculateDirectionB(time);
    101.         float directionAfter = CalculateDirectionB(time + deltaTime);
    102.         float angularVelocity = (directionAfter - directionAtTime) / deltaTime;
    103.         return angularVelocity;
    104.     }
    105.  
    106.     float CalculateDirectionB(float time)
    107.     {
    108.         Vector3 tangent = testSplineContainer.Spline.EvaluateTangent(time / simulationDuration);
    109.         float direction = Mathf.Atan2(tangent.z, tangent.x);
    110.         if(direction < 0)
    111.         {
    112.             direction += 2 * Mathf.PI;
    113.         }
    114.         return direction;
    115.     }
    116.  
    117.     private void createVisual()
    118.     {
    119.         GameObject test = GameObject.CreatePrimitive(PrimitiveType.Cube);
    120.         test.transform.localScale = new Vector3(.5f, .5f, 36);
    121.         test.transform.parent = visualizationObjectParent.transform;
    122.         test.transform.localPosition = new Vector3(0, 0, 18);
    123.         test.GetComponent<Renderer>().material.color = Color.red;
    124.     }
    125. }
    When I run the script on a simple spline, I get a result that looks pretty good - it is what I would expect: (the red lines represent the trailer at discrete intervals)


    However, when I apply this script to a spline which curves back more, it breaks:



    I'm not very strong with this sort of geometry and I have not had much luck with my trial an error approach to figuring it out over the last couple of days, so I'm hoping someone can help me understand what's going on.

    Thanks!
     
  2. orionsyndrome

    orionsyndrome

    Joined:
    May 4, 2014
    Posts:
    3,318
    I think your error is this
    Code (csharp):
    1. angleB = GetDirectionB(time + deltaTime).x; // Assuming x-axis represents the direction
    This makes no sense, why would you take only the x component? This is 2D vector, by taking only the x component, you're destroying it. The whole thing is a direction. Just remove that
    .x
    part

    I would also write many of these lines differently.
    For example (start by removing the boilerplate)
    Code (csharp):
    1. static Vector2 tov2(Vector3 v) => new Vector2(v.x, v.z);
    2. static Vector3 tov3(Vector2 v) => new Vector3(v.x, 0f, v.y);
    3.  
    4. static float sin(float rad) => MathF.Sin(rad); // System namespace; don't use Mathf for trigonometry
    5. static float cos(float rad) => MathF.Cos(rad);
    6. static Vector2 sincos(float rad) => new Vector2(sin(rad), cos(rad));
    7.  
    8. static float angle(Vector2 v) {
    9.   var theta = MathF.Atan2(v.y, v.x);
    10.   if(theta < 0f) theta += 2f * MathF.PI;
    11.   return theta;
    12. }
    13.  
    14. static float abs(float n) => Math.Abs(n);
    15. static Quaternion roty(float rad) => Quaternion.AngleAxis(rad * Mathf.Rad2Deg, Vector3.up);
    16. //static Quaternion roty(float rad) => Quaternion.Euler(0f, rad * Mathf.Rad2Deg, 0f); // ^^ much faster
    17.  
    18. Vector2 samplePos(float time)
    19.   => tov2(testSplineContainer.Spline.EvaluatePosition(time / _simDuration));
    20.  
    21. Vector2 sampleTangent(float time)
    22.   => tov2(testSplineContainer.Spline.EvaluateTangent(time / _simDuration)).normalized;
    Code (csharp):
    1. void UpdateTrailerPosition() {
    2.  
    3.   var dt = deltaTime;
    4.  
    5.   var posb = samplePos(_time);
    6.   //var dirb = sampleTangent(_time); // you never use this
    7.  
    8.   var posa = posb + (d1 + d2) * sincos(_angb); // I'm assuming this is tractor pos? you never use this either
    9.  
    10.   var psi = _angd - _angb;
    11.  
    12.   var angveld = ( abs(calcVel()) * sin(psi) + d2 * abs(calcAngVel()) * cos(psi) ) / d3;
    13.  
    14.   _angd -= angveld * dt;
    15.   _angb = angle(sampleTangent(_time + dt));
    16.  
    17.   var posd = posb + d2 * sincos(_angb) + d3 * -sincos(_angd);
    18.   _time += dt;
    19.  
    20.   visualizationObjectParent.transform.rotation = roty(_angd);
    21.   visualizationObjectParent.transform.position = tov3(posd);
    22.  
    23.   float calcVel() => (samplePos(_time) - samplePos(_time - dt)).magnitude / dt;
    24.   float calcAngVel() => (angle(sampleTangent(_time + dt)) - angle(sampleTangent(_time))) / dt;
    25.  
    26. }
    Maybe it's just me, but I can negotiate with this better.
    Once the dust settles down, you can rename / flesh out the var names for longer shelf life.

    Edit: some fixes
    Edit2: sampleTangent doesn't need to return a normalized vector, because in this particular situation, you only ever compute angles with it; atan2 is perfectly tolerant to non-unit vectors (it works with y/x ratio internally, so it doesn't matter). fyi: normalized and magnitude are more expensive than they look.
    Edit3: sample functions cannot be static; also fixed calcVel and calcAngVel being called wrongly.
     
    Last edited: Jun 7, 2023
  3. orionsyndrome

    orionsyndrome

    Joined:
    May 4, 2014
    Posts:
    3,318
    Also this was confusing
    Code (csharp):
    1. float CalculateDirectionB(float time)
    2. {
    3.   Vector3 tangent = testSplineContainer.Spline.EvaluateTangent(time / simulationDuration);
    4.   float direction = Mathf.Atan2(tangent.z, tangent.x);
    5.   if(direction < 0)
    6.   {
    7.     direction += 2 * Mathf.PI;
    8.   }
    9.   return direction;
    10. }
    This is not a direction, this is an angle. And then you're using it to extrapolate angular velocity.
    Directions are unit vectors. I've renamed this to
    angle
    above.
     
    Last edited: Jun 6, 2023
  4. orionsyndrome

    orionsyndrome

    Joined:
    May 4, 2014
    Posts:
    3,318
    Also the consistent ordering of (sin, cos) in the equations is very peculiar. Usually this kind of geometry in Unity is (cos, sin) and a perpendicular situation would be (-sin, cos) or (sin, -cos). So I think they're measuring their angles from the top, instead of from the right (which is what trig functions do). And that would be my next suspect.
     
  5. orionsyndrome

    orionsyndrome

    Joined:
    May 4, 2014
    Posts:
    3,318
    Here's a shot of the actual equations in the paper, for posterity

    upload_2023-6-7_14-52-16.png
     
    Yoreki and w_adams like this.
  6. orionsyndrome

    orionsyndrome

    Joined:
    May 4, 2014
    Posts:
    3,318
    Also observe the asymmetry between lines 23 and 24 in my version.
    Code (csharp):
    1. float calcVel() => (samplePos(_time) - samplePos(_time - dt)).magnitude / dt;
    2. float calcAngVel() => (angle(sampleTangent(_time + dt)) - angle(sampleTangent(_time))) / dt;
    These two function should sample the same time stamps, currently one is reaching to the past, and the other to the future. This is completely opaque in your coding style.

    I will try to untangle this in my spare time. This needs to be rewritten with more attention to details.

    Hopefully you'll leave a comment in the meantime.
     
  7. w_adams

    w_adams

    Joined:
    Apr 7, 2016
    Posts:
    34
    Thank you so much for your attention on this Orion. I started to get into your suggestions last night and I did a re-write based on the code you provided, but I'm a bit unfamiliar with the way you are writing things and it's taking me a bit to understand what you have suggested. You're right that my script is a bit of a mess - I was essentially just randomly trying different things to try and figure out what was going on, so it definitely needs a cleanup and a more consistent approach at this point. I'm going to be working on it today and I will update with my progress.
     
  8. orionsyndrome

    orionsyndrome

    Joined:
    May 4, 2014
    Posts:
    3,318
    Try my approach, not because it's mine, but because it's much cleaner to work with.
    I've developed this style after working with shaders a lot. This also naturally translates to Unity.Mathematics.

    You can easily discern between the symbols, acknowledge patterns and move things around / experiment.
    Bundle reusable techniques in their own functions, remove all boilerplate from sight, and develop your own static lexicon. Contrary to C# standards, the shorter the names the better; as I said, you can easily rename them for longevity when you're finished.

    If I find some time, I will definitely try to make this. But I'm in a midst of building something else entirely, and I can't tell when that'll be.
     
  9. orionsyndrome

    orionsyndrome

    Joined:
    May 4, 2014
    Posts:
    3,318
    Well I did nothing particularly extraordinary. If you have any concrete questions, feel free to ask.

    C# wise, the only features I'm using here are
    - static methods
    - expression-body definitions
    - local functions

    Geometry-wise, this is heavy on
    - basic trigonometry
    - basic linear algebra
    - a simple quaternion rotation
    - but not much else really
     
    Last edited: Jun 7, 2023
  10. w_adams

    w_adams

    Joined:
    Apr 7, 2016
    Posts:
    34
    Yeah, the expression-body definitions and local functions were something I have not used before; I can see how they improve readability and reduce the chance of unintended things happening, respectively. I also really need to improve my understanding of trigonometry to be able to better identify what is actually happening. I am wondering why you made most of the member functions static?
     
  11. orionsyndrome

    orionsyndrome

    Joined:
    May 4, 2014
    Posts:
    3,318
    Because they're pure, live outside of instance context, and are extremely likely to be inlined by compiler, when paired with an expression-body. Also traditionally, math libraries are made to be static, because these just parachute in (Math, MathF, Mathf).

    I would argue that any method that doesn't need immediate access to instance data should be
    static
    , but there are no hard rules about this, and certainly there could be some design constraints which is why there are no hard rules about this.
     
    CodeRonnie, Kurt-Dekker and w_adams like this.
  12. orionsyndrome

    orionsyndrome

    Joined:
    May 4, 2014
    Posts:
    3,318
    Also btw, historically C# didn't have a dedicated 32-bit precision math library, only a 64-bit one. This is why there is this confusion right now between Mathf and MathF. MathF has been added to the language only recently, and prior to that, the only convenient way to work with
    float
    math was Mathf made by Unity.

    However, making low-level math libraries is a huge problem, and so while most of it is ok and tied up to Unity workflows, when it comes to trigonometry, logarithms, square roots et al, all Unity does there is casting your float to double, calling a corresponding function in System.Math, then casting the result back to float. Not very good for performance, right?

    In recent times, we now have a dedicated library from Unity (Unity.Mathematics), as well as System.MathF. The performance boost can be huge.
     
  13. orionsyndrome

    orionsyndrome

    Joined:
    May 4, 2014
    Posts:
    3,318
    Also, regarding local functions, the same thing applies with them: if they're small and expression-bodied they are extremely likely to become inlined, which is what you want most of the time. Otherwise, the sacrifice would be too great in most applications. Apart from letting you reuse some code without cluttering your class member space, they also come in two flavors, static and non-static, and the difference is wild.

    Static local functions behave "normally" in a sense that they are treated as separate functions with nothing funky going on, but non-static local functions have this weird but useful feature that's introduced with lambdas: variable capturing.

    In other words, they can hook onto variables of their parent scope by reference, making them even more useful (but sometimes horrible if you don't know what you're doing). And you can always opt-out by making them static, which is pretty neat.

    I honestly can't imagine going back to times without them. I've just pondered over some of my workhorse functions in the project I'm working on, and tried to imagine how different they would look and operate without local functions. The conclusion was "a lot", though the change is not always obviously for the absolute better, they tend to be more readable, and offer a different, let's say function-oriented mind set.
     
    Last edited: Jun 7, 2023
  14. w_adams

    w_adams

    Joined:
    Apr 7, 2016
    Posts:
    34
    Yeah, I see there is some stuff there that I have been completely oblivious to. Thanks for opening my eyes to it, definitely going to start working with it
     
    orionsyndrome likes this.
  15. w_adams

    w_adams

    Joined:
    Apr 7, 2016
    Posts:
    34
    Is it your interpretation that the red line represents the spline?
     
  16. orionsyndrome

    orionsyndrome

    Joined:
    May 4, 2014
    Posts:
    3,318
    No. I haven't really made any assumptions on how you coded the equations, except that
    .x
    thing which I removed. All I did was transform your code into something more workable. Spline is not shown in that picture, btw. The tractor (A) should be on the spline, as a point, and Va should be the spline's tangent. I'm not sure about B, I would have to look at that more closely, but D is definitely not on the spline.

    Edit: See the next post for a more accurate answer.
     
    Last edited: Jun 7, 2023
  17. orionsyndrome

    orionsyndrome

    Joined:
    May 4, 2014
    Posts:
    3,318
    I've found this confusing already, why they find Pa, only to then do nothing with it. You didn't do anything either.
    After reading this, here's an explanation, this entire model is based on the rear wheels of the tractor.
    (then goes the above picture)
    So the idea is to sample B from the spline, then compute A out of that, which is how it looked like. Now you can obtain C, and D from C and the angles involved.

    So the answer is: Only the point B is exactly on the spline, though point A quite possibly tries to get onto it as well. That's the key constraint they're using.

    In any case, you most definitely want to be able to understand this geometrically and apply trigonometry on your own to solve this independently.

    This is all pretty basic math, and it's very nice that you get all the equations so you get a point of comparison.
     
    Last edited: Jun 7, 2023
  18. w_adams

    w_adams

    Joined:
    Apr 7, 2016
    Posts:
    34
    Right. I guess Pa can be used to position the tractor for animation purposes, but it's not useful in the determination of the trailer. As you just noted, the idea is to sample B from the spline, so I assume the spline would be something like this:
    trailer diagram.jpeg
    I just noticed the axis designation in the lower left corner of that diagram, so does the red line represent the y axis?
     
  19. orionsyndrome

    orionsyndrome

    Joined:
    May 4, 2014
    Posts:
    3,318
    Yes, that's roughly where spline is going through.

    Yes.

    Yes haha, I've noticed that already in post #4. It's just silly. But that explains perfectly that (sin, cos) situation. They just align their 0° to the North, instead to the East.

    When you rotate the whole image by 90° clockwise you get (cos, -sin). Anyway now you can see the advantage of having split methods for these things. You can do
    Code (csharp):
    1. static float sin(float rad) => MathF.Cos(rad);
    2. static float cos(float rad) => -MathF.Sin(rad);
    and blatantly pretend that everything's ok. You can easily fix everything up once you have math that works.

    I'd still double-check everything by hand. Such twists are notoriously hard to work with because this is pretty much a fundamental swap.
     
    Last edited: Jun 7, 2023
  20. orionsyndrome

    orionsyndrome

    Joined:
    May 4, 2014
    Posts:
    3,318
    Anyway at this point it makes sense to try and work this out backwards.



    You want to find D, and to find it you need to know C, psi, and d3. At least in my interpretation.
    C is easy if you have A, B, d1, and d2. It's just linear interpolation between the two.
    But they skip C altogether, I'm not sure why. Maybe it's left as an exercise for the reader?
    You definitely need it but they somehow get to D directly from B in equation (5).
    Quite possibly C is hidden here in the second term of that equation and the third term is the actual semi-trailer hanging out in the back.

    Then they claim psi = angb - angd and if I'm correct these are the angles of Vb and Vd against the Y axis (which is the upper horizontal line). There is some geometric principle here, so you really want to compute these angles more respectfully, and make them signed, not unsigned like you did.

    What else we need? Well you need B, which is sampled according to time.

    Then they compute angd and D for the next time step. They are using angular velocity of D to find out how much angd should change. That part is actually very clever.

    Throughout that equation (5) they're referring to angles in the future. So you need to take the sample of angb in the future, and you already have future angd from equation (4).
     
  21. orionsyndrome

    orionsyndrome

    Joined:
    May 4, 2014
    Posts:
    3,318
    All in all this should look like this
    Code (csharp):
    1. var posb = samplePos(_time);
    2. var posa = posb + (d1 + d2) * polar(_angb);
    3.  
    4. var psi = _angd - _angb;
    5.  
    6. // delta times here cancel out in the final _angd computation below
    7. var vel_b = (posb - samplePos(_time - dt)).magnitude; // this normally needs  / dt
    8. var new_angb = angle(sampleTangent(_time + dt));
    9. var omega_angb = new_angb - _angb; // this normally needs  / dt
    10.  
    11. _angd -= (vel_b * cos(psi) + d2 * abs(omega_angb) * sin(psi)) / d3; // this normally needs * dt
    12. _angb = new_angb;
    13.  
    14. var posd = samplePos(_time + dt) + d2 * polar(_angb) - d3 * polar(_angd);
    where
    Code (csharp):
    1. static Vector2 polar(float rad) => new Vector2(cos(rad), sin(rad));
    2. static float cos(float rad) => MathF.Cos(rad);
    3. static float sin(float rad) => MathF.Sin(rad);
    4. static float angle(Vector2 v) => MathF.Atan2(v.y, v.x);
    Edit: fixed vel_b.magnitude
     
    Last edited: Jun 7, 2023
  22. w_adams

    w_adams

    Joined:
    Apr 7, 2016
    Posts:
    34
    OK, so I tried implementing your most recent code and ended up with this:
    result.jpg

    Which is not there yet. I'm currently working through comparing what you did to the original equations - so far I haven't found any busts but it also takes me a bit because I'm not very agile with trig..
     
  23. w_adams

    w_adams

    Joined:
    Apr 7, 2016
    Posts:
    34
    One thing I did change is

    Code (CSharp):
    1. _angd -= (abs(vel_b) * cos(psi) + d2 * abs(omega_angb) * sin(psi)) / d3;
    to this:

    Code (CSharp):
    1. _angd -= (vel_b.magnitude * cos(psi) + d2 * abs(omega_angb) * sin(psi)) / d3;
    which I believe is what is intended
     
    orionsyndrome likes this.
  24. orionsyndrome

    orionsyndrome

    Joined:
    May 4, 2014
    Posts:
    3,318
    Ha, this actually looks pretty decent. In my experience, this is very close to the real solution.
    Try plugging a minus to the result of either of sin, cos, angle, see what happens.

    Indeed, my bad.
     
  25. w_adams

    w_adams

    Joined:
    Apr 7, 2016
    Posts:
    34
    -angle:
    -angle.jpg
    -cos:
    -cos.jpg
    -sin:
    -sin.jpg
     
  26. orionsyndrome

    orionsyndrome

    Joined:
    May 4, 2014
    Posts:
    3,318
    Hahaha that's as annoying as censored Japanese porn.
    Nah, it was a good attempt, the issue is somewhere else.

    This does highlight that weird gap between the first red line and the blue spline.
    There is some weird offset that persists.

    Well, I'll try to stare at it a bit more, but it seems staring only gets you so far.
    It would be helpful if you'd also plot the rest of the stuff, like posa and posb, that would highlight the possible miscalculations. Problems like this are always the case of some domino effect, so if you catch them early, it's part of the debugging process.
     
  27. orionsyndrome

    orionsyndrome

    Joined:
    May 4, 2014
    Posts:
    3,318
    Also try changing line 11 to
    Code (csharp):
    1. _angd += ..
    I'm just curious what would happen
     
  28. orionsyndrome

    orionsyndrome

    Joined:
    May 4, 2014
    Posts:
    3,318
    Code (csharp):
    1. var cur_posb = samplePos(_time);
    2. var cur_angb = angle(sampleTangent(_time));
    3.  
    4. var posa = cur_posb + (d1 + d2) * polar(cur_angb);
    5.  
    6. var velb = (cur_posb - _posb).magnitude;
    7. var omega_angb = abs(cur_angb - _angb);
    8. var psi = _angd - cur_angb;
    9.  
    10. _posb = cur_posb;
    11. _angb = cur_angb;
    12. _angd -= (velb * cos(psi) + d2 * omega_angb * sin(psi)) / d3;
    13.  
    14. var posd = _posb + d2 * polar(_angb) - d3 * polar(_angd);
    I don't expect this to work at all, but here I'm trying to vaguely interpret the notation used.
    For example when they write just t, they mean "current state" and when they write time+dt they mean "next state". (Also add
    _posb
    as a class member if you try this.)

    Btw line 12 might be a case of flipped sin and cos, try that as well. Edit: in fact, I'm super-confident this is the case, we did it everywhere except here. And try changing -= to +=
    Code (csharp):
    1. _angd -= (velb * sin(psi) + d2 * omega_angb * cos(psi)) / d3;
    Anyway, I tried giving it a go, but I really can't be bothered with setting it all up atm, doing the bezier path and everything. This will have to wait sadly. I've been doing gaussian distributions and I should probably focus on that while it's still fresh, because I don't like fidgeting with statistics too much.
     
    Last edited: Jun 7, 2023
  29. w_adams

    w_adams

    Joined:
    Apr 7, 2016
    Posts:
    34
    Cool, I will give that a go and I agree that plotting all of the other outputs is probably the most useful debugging approach at this time. I'll get to work on that later this evening and report back. Really appreciate your help on this, it's broken me loose from my previous non-productive approach, ha
     
    orionsyndrome likes this.
  30. w_adams

    w_adams

    Joined:
    Apr 7, 2016
    Posts:
    34
    OK, I implemented your last attempt, which resulted in this:

    updated test.jpg

    I tried the sin / cos / =+ stuff, and the results were different but not closer.

    So I started doing some debug visualization and I think there might be an issue with this function, but it looks like it should work right to me so IDK?

    Code (CSharp):
    1.     static float angle(Vector2 v) => MathF.Atan2(v.y, v.x);
    You can see the visualization code here:

    Code (CSharp):
    1.        
    2.  
    3. GameObject truckVis = createVisual(PrimitiveType.Cube, new Vector3(.5f, .5f, d3), new Vector3(0, 0, d3 / 2), Color.red);
    4.         truckVis.transform.position = tov3(posd);
    5.         truckVis.transform.rotation = roty(_angd);
    6.  
    7.         GameObject bVis = createVisual(PrimitiveType.Cube, new Vector3(.5f, .5f, 10f), new Vector3(0, 0, 5), Color.green);
    8.         bVis.transform.position = tov3(cur_posb);
    9.         bVis.transform.rotation = roty(_angb); // this doesn't work like I think it should
    10.  
    11.         GameObject bVis2 = createVisual(PrimitiveType.Cube, new Vector3(.5f, .5f, 10f), new Vector3(0, 0, 5), Color.yellow);
    12.         bVis2.transform.position = tov3(cur_posb);
    13.         bVis2.transform.rotation = Quaternion.LookRotation(tov3(sampleTangent(_time-dt))); //this is how I would expect it to work
    14.  
    and the result:

    debug.jpg

    If I understand correctly, the green lines should be the tan of the curve at it's position, just like the yellow ones.

    Anyway I need to head to bed but I'll keep working through the results in this way in the morning
     
  31. orionsyndrome

    orionsyndrome

    Joined:
    May 4, 2014
    Posts:
    3,318
    Ok, we can easily test the round-trip with this experiment (try it here)
    Code (csharp):
    1. using System;
    2.                  
    3. public class Program {
    4.  
    5.     public static void Main() {
    6.      
    7.         const int C = 24;
    8.      
    9.         for(int i = -(C >> 1); i <= (C >> 1); i++) {
    10.             var angle = i * (2f * MathF.PI / C);
    11.             Console.WriteLine($"{deg(angle)} vs {deg(atan2(polar(angle)))}");
    12.         }
    13.      
    14.         static (float x, float y) polar(float rad) => (MathF.Cos(rad), MathF.Sin(rad));
    15.         static float atan2((float x, float y) xy) => MathF.Atan2(xy.y, xy.x);
    16.         static float deg(float rad) => rad / MathF.PI * 180f;
    17.  
    18.     }
    19.  
    20. }
    Code (csharp):
    1. -180 vs 179.99998
    2. -165 vs -165
    3. -150 vs -150
    4. -135 vs -135
    5. -120 vs -120
    6. -105.00001 vs -105.00001
    7. -90 vs -90
    8. -75 vs -75
    9. -60 vs -60
    10. -45 vs -45
    11. -30 vs -30
    12. -15 vs -15
    13. 0 vs 0
    14. 15 vs 15
    15. 30 vs 30
    16. 45 vs 45
    17. 60 vs 60
    18. 75 vs 75
    19. 90 vs 90
    20. 105.00001 vs 105.00001
    21. 120 vs 120
    22. 135 vs 135
    23. 150 vs 150
    24. 165 vs 165
    25. 180 vs -179.99998
    This doesn't appear to be the problem.

    Have you tried the suggestion(s) I made in post #28?

    Edit:
    Ah ok, I just saw this
     
    Last edited: Jun 8, 2023
  32. orionsyndrome

    orionsyndrome

    Joined:
    May 4, 2014
    Posts:
    3,318
    To debug this properly, start with doing just this
    Code (csharp):
    1. var cur_posb = samplePos(_time);
    2. var cur_angb = angle(sampleTangent(_time));
    3.  
    4. var posa = cur_posb + (d1 + d2) * polar(cur_angb);
    First verify that A lands where it's supposed to land. This should be easy, you more or less know exactly where it's supposed to be: on the tangent at the distance of (d1 + d2)

    Once you get that right, you can tell that both sample functions as well as
    angle
    do their job.

    Then proceed with this
    Code (csharp):
    1. var velb = (cur_posb - _posb).magnitude;
    2. var omega_angb = abs(cur_angb - _angb);
    3. var psi = _angd - cur_angb;
    These should be right, because there is nothing special about them.
    (Maybe
    psi
    is inverted for some reason, but that's it.)

    And finally, you should be able to nail this down, just by monkey-typing if all else fails
    Code (csharp):
    1. _posb = cur_posb;
    2. _angb = cur_angb;
    3. _angd -= (velb * cos(psi) + d2 * omega_angb * sin(psi)) / d3;
    4. var posd = _posb + d2 * polar(_angb) - d3 * polar(_angd);
    The first two lines don't matter, and with the latter two it's just the matter of signs and sin/cos combinations at this point. I'm absolutely certain the last line is practically B + d2*Vb - d3*Vd = C - d3*Vd so you can treat the terms completely separately (and you can also visualize C and confirm this part).
    Code (csharp):
    1. var posc = _posb + d2 * polar(_angb);
    2. var posd = posc - d3 * polar(_angd);
    Now you have all 4 points: A, B, C, D.
     
  33. w_adams

    w_adams

    Joined:
    Apr 7, 2016
    Posts:
    34
    OK, I only had a little time to work on this this evening, but I started with posb, angb, and pos a.

    What I am struggling a bit with is not just what are the results of the debugging, but what should the correct results look like and how does the debugging differ. So I decided it might help me to try and reproduce their diagram a bit so I can compare.

    posA.jpg

    To start, posa is the blue dot, posb the cyan dot, and angb the cyan arm.

    it does seem like posa is landing in a reasonable spot, but angb seems wrong. I believe it should pass through point A, no? I am a bit confused by what angb should be rotating to and from, which is why I put in the white line (Z axis), which I believe is our version of the Y axis in their diagram. Or maybe angb is actually correct and I am just expecting the wrong thing. I'm a bit confused.

    Code (CSharp):
    1.        
    2. GameObject bvis = createVisual(PrimitiveType.Cube, new Vector3(.5f, .5f, 50f), new Vector3(0, 0, 25f), Color.cyan, true);
    3.         bvis.transform.position = tov3(cur_posb);
    4.         bvis.transform.rotation = roty(cur_angb);
    5.  
    6.         GameObject avis = createVisual(PrimitiveType.Sphere, new Vector3(2,2,2), Vector3.zero, Color.blue, false);
    7.         avis.transform.position = tov3(_posa);
    8.  
    I should be able to spend most of the morning on this tomorrow, I plan to keep trying this approach.
     
  34. orionsyndrome

    orionsyndrome

    Joined:
    May 4, 2014
    Posts:
    3,318
    Good setup, but you're misleading yourself with faulty visualization.
    How can cur_angb be wrong if correct posa is derived from it? (That was the point of these steps.)

    You're transforming the angle wrongly somehow and then rotating the rod by this wrong amount.
    Or, the rod's identity orientation is wrong, and then you're rotating this by a correct amount.

    You're supposed to instead compute the point directly by doing
    Code (csharp):
    1. cur_posb + polar(cur_angb)
    (Which is what posa already does and so it has to be correct).
    This would give you a segment that is made from a unit direction vector, but offset to begin at B.
    posa is this same concept, only sets the length of the vector to (d1 + d2) which is correct according to diagram.

    The way I make gizmo lines for debugging is by computing the segments directly, I don't stretch them or rotate them, I just span the segments between raw points. You can learn more about this here and here. The former tutorial is especially gentle at showing this in more detail.
     
  35. orionsyndrome

    orionsyndrome

    Joined:
    May 4, 2014
    Posts:
    3,318
    I don't know what the parameters here are
    Code (csharp):
    1. createVisual(PrimitiveType.Cube, new Vector3(.5f, .5f, 50f), new Vector3(0, 0, 25f), Color.cyan, true)
    But this is a forward vector (0, 0, 25), but I'm guessing you're on the XZ plane, so it's technically an up vector in 2D. And you then apply a correct rotation with roty(cur_angb).

    Effectively you're rotating every line by 90 - angle degrees, which is quite funky, and the reason why you can't fix it just by switching signs. Look at the picture again, the angles are good they just go from the vertical CW, instead from the horizontal CCW.

    To have an identity rotation you need a right vector (25, 0, 0). Ideally you don't scale these either, you just use
    Vector2.right
    (or Vector3 in your case). Which would make this error more apparent. And no need to rotate them later, just transform the vector immediately:
    roty(cur_angb) * Vector3.right
    would give you a correct point which you then simply offset by B. (Or as I said before just do
    tov3(polar(cur_angb))
    , these are exact same things, the former is derived from a quaternion, the latter from trigonometry. This is why I call this function polar.)

    Observe 0° in this image
     
    Last edited: Jun 9, 2023
  36. orionsyndrome

    orionsyndrome

    Joined:
    May 4, 2014
    Posts:
    3,318
    I guess it's TIL time!

    (quaternion, quaternion) and (quaternion, vector3) multiplications are staple operation in Unity allowing you to apply the left-hand rotation to the right hand side. They are non-commutative so it must be in this order.

    Both are covered by this article.

    This are well-known operations in mathematics, however hard to nail right in Unity because of its left-handedness and many moving parts and conventions.

    Thanks to Unity.Mathematics being open we can see exactly what's going on

    Code (csharp):
    1. public static float3 mul(quaternion q, float3 v) {
    2.   float3 t = 2 * cross(q.value.xyz, v);
    3.   return v + q.value.w * t + cross(q.value.xyz, t);
    4. }
    Where cross does a series of perpdots across the planes (perpdot being dot(a, perp(b)); where perp(v) = (-v.y, v.x)) ending up as
    Code (csharp):
    1. static public float3 cross(float3 a, float3 b)
    2.   => new float3(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);
    If you wonder how there is no trigonometry anywhere to be seen, this is because trigonometry is implied. It was actually computed on assembling the quaternion.

    This is what angle axis does
    Code (csharp):
    1. public static quaternion AxisAngle(float3 axis, float angle) {
    2.   float sina, cosa;
    3.   math.sincos(0.5f * angle, out sina, out cosa);
    4.   return quaternion(float4(axis * sina, cosa));
    5. }
    And so effectively when you do roty(angle) * Vector3.right
    you in fact do (0, sin(angle/2), 0, cos(angle/2)) * (1, 0, 0)

    and then t = 2 * cross((0, sin(angle/2), 0), (1, 0, 0)) = (0, 0, -2 * sin(angle/2))
    to be finished with (1, 0, 0) + q.w * t + cross(q.xyz, t);

    through various sin^2 identities this will end up being the same as
    (1 * cos(angle), 0, 1 * sin(angle))

    now you know why tov3(polar(angle)) is preferable in 2D :)
     
    w_adams likes this.
  37. w_adams

    w_adams

    Joined:
    Apr 7, 2016
    Posts:
    34
    OK, progress. I did what you recommended and started using gizmo lines for debugging based on your crazy tutorial where the dude almost died :D. Positions A, B, and C all look good. However, psi and position d are not looking good. They are pretty dependent on each other, so I'm not sure what's going on yet, but at least it's getting narrowed down...

    Code (CSharp):
    1.         drawSegment(_posb, posc, _green, 3f);
    2.         drawSegment(posc, posa, _orange, 3f);
    3.         drawSegment(posc, posc + polar(psi), _rviolet, 5f);
    4.         drawSegment(posc, posd, _rorange);
    psi issue.jpg
     
  38. w_adams

    w_adams

    Joined:
    Apr 7, 2016
    Posts:
    34
    I just saw your new post. Digesting...
     
  39. orionsyndrome

    orionsyndrome

    Joined:
    May 4, 2014
    Posts:
    3,318
    Can't see what could be wrong.
    Maybe I interpreted this poorly. Try this
    Code (csharp):
    1. _angd -= abs((velb * cos(psi) + d2 * omega_angb * sin(psi))) / d3;
    also try swapping sin and cos here. We pretty much don't know whether it should be sin, cos, or cos, sin unless we unpack the geometric meaning behind this. That would mean solving the whole thing all over.

    Also make sure d1, d2, d3 are never negative numbers. Saying just in case.

    Edit:
    Another way to interpret equation (3) is that they don't tell you what the sign is supposed to be, they just give angd's quantity. I'm not sure how exactly to interpret the vertical bars on the left side of the equation.
     
    Last edited: Jun 9, 2023
  40. orionsyndrome

    orionsyndrome

    Joined:
    May 4, 2014
    Posts:
    3,318
    But they are not the same. I think the difference should be mild if the curvature of the bezier is large enough.
    Still, the semi-trailer should move naturally when you play this as animation. That's the best litmus test.
     
  41. w_adams

    w_adams

    Joined:
    Apr 7, 2016
    Posts:
    34
    Yeah, I don't understand the bars on the left side of eq 3 either.

    I tried your new interpretation of the equation, with results not much different:
    frame 1.jpg
    frame 2.jpg

    and also tried swapping sin / cos, with differences but not that I could determine looked better.

    I am starting to wonder about these equations, are they correct? Or maybe are they intended to be used in a way that is different from this? It is weird that when you increase velocity, it significantly impacts the trailer position (in a real world situation you would expect that there would be some impact based on physics, but it seems to me that the equations only are using velocity as a trick to get deltas.) Violet line represents velb

    frame 3.jpg
    frame 4.jpg
     
  42. orionsyndrome

    orionsyndrome

    Joined:
    May 4, 2014
    Posts:
    3,318
    I think I figured out what it means. Notice the little arrow? That just means "this is the magnitude of this (angular velocity) vector" and I'm 100% sure that it does go negative to indicate the winding. So remove abs.

    Code (csharp):
    1. _angd -= (velb * sin(psi) + d2 * omega_angb * cos(psi)) / d3;
    I also think I've managed to decipher what they're doing here exactly. And the more I look at it the less I see any problems with it. I think it should definitely go sin then cos, because sin goes along with the velocity like it does with torque. And I think it additionally receives the angular velocity of A-B but conserves the total energy of the motion energy by scaling it with the magnitude of the leftover component. This produces the characteristic rotation that is also inversely proportional to the length of the semi-trailer: the longer it is the less it reacts.


    τ = |r| |F⊥| = |r| |F| sin θ

    However it's apparent that the angle should go positive when F⊥ is winds CCW, however this is also a right-hand coordinate system and here torque goes toward you. So I'm beginning to think: wait, torque is not a 2D vector, so here the handedness plays a role -- WHAT IF the whole diagram is right-handed and this is why they subtract from the angle?

    If we shift the sign like this
    Code (csharp):
    1. _angd += -(velb * sin(psi) + d2 * omega_angb * cos(psi)) / d3;
    2. _angd += (velb * -sin(psi) + d2 * omega_angb * -cos(psi)) / d3
    Both sin and cos turn negative.

    And so while the CCW velocity produces positive angle change, the rotation of angb produce should remain negative to satisfy the inverted chirality.

    So maybe this should look like this instead
    Code (csharp):
    1. _angd += (velb * sin(psi) - d2 * omega_angb * cos(psi)) / d3;
    I know. Such a twist!

    I'm not sure if this affects the last equation. Or if it's correct, for that matter.
     
  43. orionsyndrome

    orionsyndrome

    Joined:
    May 4, 2014
    Posts:
    3,318
    I have good news.
    Code (csharp):
    1. using System;
    2. using UnityEngine;
    3. using UnityEditor;
    4. using EditorToolkit;
    5. using static EditorToolkit.SimpleSceneViewPanel;
    6.  
    7. [ExecuteInEditMode]
    8. public class SemiTrailer : MonoBehaviour {
    9.  
    10.   [SerializeField] [Range(0f, 20f)] float _d1 = 4f;
    11.   [SerializeField] [Range(0f, 20f)] float _d2 = 2.5f;
    12.   [SerializeField] [Range(0f, 20f)] float _d3 = 16f;
    13.   [SerializeField] [Range(.01f, 3f)] float _width = 0.6f;
    14.   [SerializeField] [Range(0f, 2f)] float _gizmoScale = 1f;
    15.  
    16.   //-----------------------------------
    17.  
    18.   SimpleSceneViewPanel _panel;
    19.  
    20.   double _start_time;
    21.   double _last_time;
    22.  
    23.   Vector3[] _mpw;
    24.  
    25.   Vector2[] _last_B;
    26.   float _last_angB = 0f;
    27.   float _last_angD = 0f;
    28.   float _saved_psi;
    29.  
    30.   //-----------------------------------
    31.  
    32.   Color _c1 = hexColor(0xedad09);
    33.   Color _c2 = hexColor(0x09bced);
    34.   Color _c3 = hexColor(0x096ced);
    35.   Color _c4 = hexColor(0xad09ed);
    36.  
    37.   //-----------------------------------
    38.  
    39.   void OnEnable() {
    40.     _mpw = new Vector3[12];
    41.     _last_B = new Vector2[24];
    42.     _panel = new SimpleSceneViewPanel("Info", 160f, rows: 4, panelInfo, ScreenCorner.BottomLeft);
    43.     SceneView.duringSceneGui += OnSceneGUI;
    44.     _start_time = Time.realtimeSinceStartupAsDouble;
    45.   }
    46.  
    47.   void OnDisable() {
    48.     destroyPanel();
    49.     SceneView.duringSceneGui -= OnSceneGUI;
    50.   }
    51.  
    52.   //-----------------------------------
    53.  
    54.   void OnDrawGizmos() {
    55.     var time = Time.realtimeSinceStartupAsDouble - _start_time;
    56.     var dt = (float)(time - _last_time);
    57.  
    58.     var B = Vector2.Lerp(_last_B[^1], v2(avg(_mpw)), 5f * dt);
    59.  
    60.     var velB = (B - avg(_last_B)); // / dt;
    61.     var tanB = velB.normalized;
    62.     var angB = ang(tanB);
    63.     var omgB = dang(_last_angB, angB); // / dt;
    64.  
    65.     var A = mad(_d1 + _d2, tanB, B);
    66.  
    67.     var psi = _saved_psi = dang(angB, _last_angD);
    68.  
    69.     var omgD = (velB.magnitude * sin(psi) + _d2 * omgB * cos(psi)) / _d3;
    70.     var angD = moda(_last_angD - omgD); // * dt;
    71.  
    72.     if(float.IsNaN(angD)) angD = angB;
    73.  
    74.     var C = mad(_d2, polar(angB), B);
    75.     var D = mad(_d3, -polar(angD), C);
    76.  
    77.     drawPoint(A, _c1);
    78.     drawSeg(A, C, _c1);
    79.     drawPoint(B, _c2);
    80.     drawSeg(B, C, _c2);
    81.     drawPoint(C, _c3);
    82.     drawSeg(C, D, _c3);
    83.     drawPoint(D, _c4);
    84.  
    85.     var w = (C - A).magnitude * _width;
    86.     drawRect2((A + C) / 2f, (B - A).normalized, new Vector2(w, 2f * w), Color.green, thickness: 1f);
    87.     drawRect2(((C + D) - (C - D).normalized * _d2) / 2f, (C - D).normalized, new Vector2(w, _d3), Color.grey, thickness: 1f);
    88.  
    89.     _last_time = time;
    90.     _last_angB = angB;
    91.     _last_angD = angD;
    92.  
    93.     addToSeries(_last_B, B);
    94.   }
    95.  
    96.   //-----------------------------------
    97.  
    98.   void addToSeries<T>(T[] ary, T value) where T : struct {
    99.     for(int i = 1; i < ary.Length; i++) ary[i-1] = ary[i];
    100.     ary[^1] = value;
    101.   }
    102.  
    103.   static Vector2 avg(Vector2[] ary, int skip = 1)
    104.     => avg(ary, max(1, skip), (sum, item) => sum + item, (sum, count) => sum / count );
    105.  
    106.   static Vector3 avg(Vector3[] ary, int skip = 1)
    107.     => avg(ary, max(1, skip), (sum, item) => sum + item, (sum, count) => sum / count );
    108.  
    109.   static T avg<T>(T[] ary, int skip, Func<T, T, T> add, Func<T, float, T> div) where T : struct {
    110.     var sum = default(T);
    111.     int count = 0;
    112.     for(int i = 0; i < ary.Length; i += skip) { sum = add(sum, ary[i]); count++; }
    113.     return div(sum, count);
    114.   }
    115.  
    116.   //-----------------------------------
    117.  
    118.   void OnSceneGUI(SceneView view) {
    119.     var e = Event.current;
    120.  
    121.     if(e.isMouse) {
    122.       addToSeries(_mpw, toWorldPoint(e.mousePosition));
    123.       if(_panel.Visible) view.Repaint();
    124.     }
    125.   }
    126.  
    127.   Vector3 toWorldPoint(Vector2 pos) {
    128.     var ray = HandleUtility.GUIPointToWorldRay(pos);
    129.     var plane = new Plane(Vector3.back, Vector3.zero);
    130.     plane.Raycast(ray, out var result);
    131.     return mad(result, ray.direction, ray.origin);
    132.   }
    133.  
    134.   void destroyPanel() {
    135.     if(_panel is not null) {
    136.       _panel.Enabled = false;
    137.       _panel = null;
    138.     }
    139.   }
    140.  
    141.   PanelRow panelInfo(int row)
    142.     => row switch {
    143.          0 => PanelRow.Label($"Mouse: {v2(_mpw[^1]):F3}"),
    144.          1 => PanelRow.Label($"Angle B: {deg(_last_angB):F2}°"),
    145.          2 => PanelRow.Label($"Angle D: {deg(_last_angD):F2}°"),
    146.          3 => PanelRow.Label($"Psi: {deg(_saved_psi):F2}°"),
    147.          _ => null
    148.        };
    149.  
    150.   //-----------------------------------
    151.  
    152.   void drawSeg(Vector2 a, Vector2 b, Color? color = null, float thickness = 2f) {
    153.     if(color.HasValue) Handles.color = color.Value;
    154.     Handles.DrawLine(v3(a), v3(b), thickness * _gizmoScale);
    155.   }
    156.  
    157.   void drawCircle(Vector2 c, float r, int segments = 48, Color? color = null, float thickness = 2f) {
    158.     if(color.HasValue) Handles.color = color.Value;
    159.     var last = Vector2.zero;
    160.     var step = TAU / segments;
    161.     for(int i = 0; i <= segments; i++) {
    162.       var p = mad(r, polar((float)i * step), c);
    163.       if(i > 0) drawSeg(last, p, thickness: thickness);
    164.       last = p;
    165.     }
    166.   }
    167.  
    168.   void drawPoint(Vector2 p, Color? color = null)
    169.     => drawCircle(p, .04f * _gizmoScale, segments: 8, color, thickness: 3f);
    170.  
    171.   void drawRect2(Vector2 c, Vector2 dir, Vector2 size, Color? color = null, float thickness = 1f) {
    172.     var perp = new Vector2(-dir.y, dir.x);
    173.     var extent = (-size.x * perp + size.y * dir) * .5f;
    174.     drawRect(c + extent, c - extent, -ang(dir), color, thickness);
    175.   }
    176.  
    177.   void drawRect(Vector2 a, Vector2 b, float rot, Color? color = null, float thickness = 1f) {
    178.     const float SQRT2_2 = 0.70710678f;
    179.     const float PI_2 = MathF.PI / 2f;
    180.  
    181.     if(color.HasValue) Handles.color = color.Value;
    182.     var q = rot == 0f? qid : angleaxis(rot, Vector3.back);
    183.     var iq = invq(q);
    184.     a = iq * a; b = iq * b;
    185.     var r = encaps(a, b);
    186.  
    187.     var last = Vector2.zero;
    188.     for(int i = 0; i <= 4; i++) {
    189.       var p = q * mad(SQRT2_2 * r.size, polar(((float)i + .5f) * PI_2), r.center);
    190.       if(i > 0) drawSeg(last, p, thickness: thickness);
    191.       last = p;
    192.     }
    193.   }
    194.  
    195.   Rect encaps(Vector2 a, Vector2 b) {
    196.     Vector2 min = leastOf(a, b), max = mostOf(a, b);
    197.     return new Rect(min, max - min);
    198.   }
    199.  
    200.   //-----------------------------------
    201.  
    202.   static readonly float PI = MathF.PI;
    203.   static readonly float TAU = 2f * PI;
    204.   static readonly Quaternion qid = Quaternion.identity;
    205.  
    206.   static float sin(float rad) => MathF.Sin(rad);
    207.   static float cos(float rad) => MathF.Cos(rad);
    208.   static Vector2 polar(float rad) => new Vector2(cos(rad), sin(rad));
    209.  
    210.   static float abs(float n) => Math.Abs(n);
    211.   static float sqrt(float n) => MathF.Sqrt(n);
    212.   static float sqrhypot(float x, float y) => x * x + y * y;
    213.  
    214.   static Vector2 mad(Vector2 a, Vector2 b, Vector2 c) => new Vector2(a.x * b.x + c.x, a.y * b.y + c.y);
    215.   static Vector2 mad(float a, Vector2 b, Vector2 c) => a * b + c;
    216.   static Vector3 mad(float a, Vector3 b, Vector3 c) => a * b + c;
    217.  
    218.   static Vector2 v2(float n) => new Vector2(n, n);
    219.   static Vector2 v2(Vector3 v) => new Vector2(v.x, v.y);
    220.   static Vector3 v3(Vector2 v) => new Vector3(v.x, v.y, 0f);
    221.  
    222.   static float min(float a, float b) => MathF.Min(a, b);
    223.   static float max(float a, float b) => MathF.Max(a, b);
    224.  
    225.   static int max(int a, int b) => Math.Max(a, b);
    226.  
    227.   static float ang(Vector2 v) => MathF.Atan2(v.y, v.x);
    228.   static float moda(float a) => (a %= TAU) < 0f? a + TAU : a;
    229.   static float dang(float a, float b) { var d = moda(b - a); return d < PI? d : d - TAU; }
    230.  
    231.   static float deg(float rad) => rad * (180f / PI);
    232.   static float rad(float deg) => deg * (PI / 180f);
    233.  
    234.   static Color32 c32lambda(Func<int, byte> lambda)
    235.     => new Color32(lambda(0), lambda(1), lambda(2), lambda(3));
    236.  
    237.   static Color32 hexColor(uint value, byte alpha = byte.MaxValue)
    238.     => c32lambda( i => (i < 3)? (byte)( ( value >> ((2-i)<<3) ) & byte.MaxValue )
    239.                               : alpha );
    240.  
    241.   static Vector2 leastOf(Vector2 a, Vector2 b) => new Vector2(min(a.x, b.x), min(a.y, b.y));
    242.   static Vector2 mostOf(Vector2 a, Vector2 b) => new Vector2(max(a.x, b.x), max(a.y, b.y));
    243.  
    244.   static Quaternion normalize(Quaternion q) {
    245.     var im = 1f / sqrt(sqrhypot(q.x, q.y) + sqrhypot(q.z, q.w));
    246.     return new Quaternion(q.x * im, q.y * im, q.z * im, q.w * im);
    247.   }
    248.  
    249.   static Quaternion angleaxis(float angle, Vector3 axis) {
    250.     const float ZERO = 3E-8f;
    251.     if(axis.sqrMagnitude < ZERO) return Quaternion.identity;
    252.     var ha = angle * .5f; axis *= sin(ha);
    253.     return normalize(new Quaternion(axis.x, axis.y, axis.z, cos(ha)));
    254.   }
    255.  
    256.   static Quaternion invq(Quaternion q) => Quaternion.Inverse(q);
    257.  
    258. }

    It took me about 3 1/2 hrs to make it and so it's 3 AM now, and I don't have much energy to explain anything. I didn't do anything too special and we can go over it in the following days.

    In short, I was extremely lazy to set up Bezier curves, so I hooked it to the editor mouse, which was a pain because it was extremely jittery, so I spent a lot of time smoothing and filtering the input. I actually used one of @Kurt-Dekker 's low-pass tricks for the final pass. It's still a little wobbly, but whatever. The equations worked out of box for me. The core of it is in OnDrawGizmos.

    Oh, make sure to also grab SimpleSceneViewPanel that I showcased recently, I use that to show transient info.

    It's far from perfect, mostly because there is no real continuity of motion unless you have a very steady hand (just trace an imaginary curve with the mouse; btw if you make a full stop it will snap into oblivion). But it serves well as a proof of concept. The math does work and you can play with the parameters.

    upload_2023-6-11_3-16-9.png
     
  44. w_adams

    w_adams

    Joined:
    Apr 7, 2016
    Posts:
    34
    Amazing! OK, I am going to grab what you did and adapt it to a non-hacky spline. FWIW, I just have been using the new built-in splines that come with unity 2022+, they are trivial to use. I'll post my working code with it once I get it running
     
  45. w_adams

    w_adams

    Joined:
    Apr 7, 2016
    Posts:
    34
    Code (CSharp):
    1.  
    2. using UnityEngine;
    3. using UnityEngine.Splines;
    4. using System;
    5. using UnityEditor;
    6.  
    7.  
    8. [ExecuteInEditMode]
    9. public class TrailerMovement : MonoBehaviour
    10. {
    11.     public SplineContainer testSplineContainer;
    12.  
    13.     public float d1 = 6.2f;
    14.     public float d2 = .6f;
    15.     public float d3 = 33.4f;
    16.     public float deltaTime = 0.5f;
    17.  
    18.     [SerializeField] [Range(0f, 50f)] private float time = 0f;
    19.     [SerializeField] [Range(0f, 5f)] private float _gizmoScale = 1f;
    20.     [SerializeField] [Range(.01f, 3f)] float _width = 0.6f;
    21.  
    22.     public float _simDuration = 50f;
    23.  
    24.     Vector2[] _last_B;
    25.     float _last_angB = 0f;
    26.     float _last_angD = 0f;
    27.     float _saved_psi;
    28.  
    29.     Color _green = hexColor(0x6cff6f);
    30.     Color _orange = hexColor(0xffb36e);
    31.     Color _blue = hexColor(0x6e83ff);
    32.     Color _rorange = hexColor(0xd39629);
    33.     Color _rviolet = hexColor(0x3e2ad1);
    34.  
    35.     private void OnEnable()
    36.     {
    37.         _last_B = new Vector2[24];
    38.     }
    39.  
    40.     void OnDrawGizmos()
    41.     {
    42.         UpdateTrailer();
    43.     }
    44.  
    45.     static Color32 c32lambda(Func<int, byte> lambda)
    46.   => new Color32(lambda(0), lambda(1), lambda(2), lambda(3));
    47.     static Color32 hexColor(uint value, byte alpha = byte.MaxValue)
    48.   => c32lambda(i => (i < 3) ? (byte)((value >> ((2 - i) << 3)) & byte.MaxValue)
    49.                            : alpha);
    50.  
    51.     static readonly float PI = MathF.PI;
    52.     static readonly float TAU = 2f * PI;
    53.     static readonly Quaternion qid = Quaternion.identity;
    54.  
    55.     static Vector2 tov2(Vector3 v) => new Vector2(v.x, v.z);
    56.     static Vector2 polar(float rad) => new Vector2(cos(rad), sin(rad));
    57.     static float cos(float rad) => MathF.Cos(rad);
    58.     static float sin(float rad) => MathF.Sin(rad);
    59.  
    60.     static Vector3 v3(Vector2 v) => new Vector3(v.x, 0f, v.y);
    61.     static float min(float a, float b) => MathF.Min(a, b);
    62.     static int max(int a, int b) => Math.Max(a, b);
    63.     static float max(float a, float b) => MathF.Max(a, b);
    64.  
    65.     static float ang(Vector2 v) => MathF.Atan2(v.y, v.x);
    66.     static float moda(float a) => (a %= TAU) < 0f ? a + TAU : a;
    67.     static float dang(float a, float b) { var d = moda(b - a); return d < PI ? d : d - TAU; }
    68.     static float sqrt(float n) => MathF.Sqrt(n);
    69.     static float sqrhypot(float x, float y) => x * x + y * y;
    70.  
    71.     static Vector2 mad(Vector2 a, Vector2 b, Vector2 c) => new Vector2(a.x * b.x + c.x, a.y * b.y + c.y);
    72.     static Vector2 mad(float a, Vector2 b, Vector2 c) => a * b + c;
    73.     static Vector2 leastOf(Vector2 a, Vector2 b) => new Vector2(min(a.x, b.x), min(a.y, b.y));
    74.     static Vector2 mostOf(Vector2 a, Vector2 b) => new Vector2(max(a.x, b.x), max(a.y, b.y));
    75.  
    76.     static Quaternion invq(Quaternion q) => Quaternion.Inverse(q);
    77.     static Quaternion angleaxis(float angle, Vector3 axis)
    78.     {
    79.         const float ZERO = 3E-8f;
    80.         if (axis.sqrMagnitude < ZERO) return Quaternion.identity;
    81.         var ha = angle * .5f; axis *= sin(ha);
    82.         return normalize(new Quaternion(axis.x, axis.y, axis.z, cos(ha)));
    83.     }
    84.     static Quaternion normalize(Quaternion q)
    85.     {
    86.         var im = 1f / sqrt(sqrhypot(q.x, q.y) + sqrhypot(q.z, q.w));
    87.         return new Quaternion(q.x * im, q.y * im, q.z * im, q.w * im);
    88.     }
    89.  
    90.     void addToSeries<T>(T[] ary, T value) where T : struct
    91.     {
    92.         for (int i = 1; i < ary.Length; i++) ary[i - 1] = ary[I];[/I]
    93.         ary[^1] = value;
    94.     }
    95.     static Vector2 avg(Vector2[] ary, int skip = 1)
    96.       => avg(ary, max(1, skip), (sum, item) => sum + item, (sum, count) => sum / count);
    97.     static T avg<T>(T[] ary, int skip, Func<T, T, T> add, Func<T, float, T> div) where T : struct
    98.     {
    99.         var sum = default(T);
    100.         int count = 0;
    101.         for (int i = 0; i < ary.Length; i += skip) { sum = add(sum, ary); count++; }
    102.         return div(sum, count);
    103.     }
    104.     Vector2 samplePos(float time)
    105.       => tov2(testSplineContainer.Spline.EvaluatePosition(time / _simDuration));
    106.  
    107.     void drawSeg(Vector2 a, Vector2 b, Color? color = null, float thickness = 2f)
    108.     {
    109.         if (color.HasValue) Handles.color = color.Value;
    110.         Handles.DrawLine(v3(a), v3(b), thickness * _gizmoScale);
    111.     }
    112.  
    113.     void drawCircle(Vector2 c, float r, int segments = 48, Color? color = null, float thickness = 2f)
    114.     {
    115.         if (color.HasValue) Handles.color = color.Value;
    116.         var last = Vector2.zero;
    117.         var step = TAU / segments;
    118.         for (int i = 0; i <= segments; i++)
    119.         {
    120.             var p = mad(r, polar((float)i * step), c);
    121.             if (i > 0) drawSeg(last, p, thickness: thickness);
    122.             last = p;
    123.         }
    124.     }
    125.  
    126.     void drawPoint(Vector2 p, Color? color = null)
    127.     => drawCircle(p, .04f * _gizmoScale, segments: 8, color, thickness: 3f);
    128.  
    129.     void drawRect2(Vector2 c, Vector2 dir, Vector2 size, Color? color = null, float thickness = 1f)
    130.     {
    131.         var perp = new Vector2(-dir.y, dir.x);
    132.         var extent = (-size.x * perp + size.y * dir) * .5f;
    133.         drawRect(c + extent, c - extent, -ang(dir), color, thickness);
    134.     }
    135.  
    136.     void drawRect(Vector2 a, Vector2 b, float rot, Color? color = null, float thickness = 1f)
    137.     {
    138.         const float SQRT2_2 = 0.70710678f;
    139.         const float PI_2 = MathF.PI / 2f;
    140.  
    141.         if (color.HasValue) Handles.color = color.Value;
    142.         var q = rot == 0f ? qid : angleaxis(rot, Vector3.back);
    143.         var iq = invq(q);
    144.         a = iq * a; b = iq * b;
    145.         var r = encaps(a, b);
    146.  
    147.         var last = Vector2.zero;
    148.         for (int i = 0; i <= 4; i++)
    149.         {
    150.             var p = q * mad(SQRT2_2 * r.size, polar(((float)i + .5f) * PI_2), r.center);
    151.             if (i > 0) drawSeg(last, p, thickness: thickness);
    152.             last = p;
    153.         }
    154.     }
    155.  
    156.     Rect encaps(Vector2 a, Vector2 b)
    157.     {
    158.         Vector2 min = leastOf(a, b), max = mostOf(a, b);
    159.         return new Rect(min, max - min);
    160.     }
    161.  
    162.     private void UpdateTrailer()
    163.     {
    164.         var dt = deltaTime;
    165.  
    166.         var B = samplePos(time);
    167.  
    168.         var velB = (B - avg(_last_B)); // / dt;
    169.         var tanB = velB.normalized;
    170.         var angB = ang(tanB);
    171.         var omgB = dang(_last_angB, angB); // / dt;
    172.  
    173.         var A = mad(d1 + d2, tanB, B);
    174.  
    175.         var psi = _saved_psi = dang(angB, _last_angD);
    176.  
    177.         var omgD = (velB.magnitude * sin(psi) + d2 * omgB * cos(psi)) / d3;
    178.         var angD = moda(_last_angD - omgD); // * dt;
    179.  
    180.         if (float.IsNaN(angD)) angD = angB;
    181.  
    182.         var C = mad(d2, polar(angB), B);
    183.         var D = mad(d3, -polar(angD), C);
    184.  
    185.         drawPoint(A, _green);
    186.         drawSeg(A, C, _green);
    187.         drawPoint(B, _orange);
    188.         drawSeg(B, C, _orange);
    189.         drawPoint(C, _blue);
    190.         drawSeg(C, D, _blue);
    191.         drawPoint(D, _rviolet);
    192.  
    193.         var w = (C - A).magnitude * _width;
    194.         drawRect2((A + C) / 2f, (B - A).normalized, new Vector2(w, 2f * w), Color.green, thickness: 1f);
    195.         drawRect2(((C + D) - (C - D).normalized * d2) / 2f, (C - D).normalized, new Vector2(w, d3), Color.grey, thickness: 1f);
    196.  
    197.         _last_angB = angB;
    198.         _last_angD = angD;
    199.  
    200.         addToSeries(_last_B, B);
    201.     }
    202. }
    203.  

    Nice work. It looks great, very satisfying. In this version I left out your panel, just so someone can grab this script and have it work standalone. Looking forward to some explanation on what you did.

    I will tweak this over the next couple of days to make this a little more robust. I want to test it in some different situations, but to me the trailer looks pretty good - the key point to watch is the end of the trailer, it should not translate to the side, it should only pivot and travel in the direction the tractor is going. This seems to achieve this in my minimal testing so far.

    trailer_1.gif
     
    Last edited: Jun 11, 2023
    orionsyndrome likes this.
  46. orionsyndrome

    orionsyndrome

    Joined:
    May 4, 2014
    Posts:
    3,318
    Looks good. It's behaving properly.

    I believe this solution only works reliably in the context of C² and G¹ curves (see more here). This is important because you use splines to essentially record the underlying physical motion. If you have a more free-form setup (like I did), then all this trigonometry and determinism becomes a chore, and it's better to use something approximate and constrained by distance: i.e. Verlet integration would do the trick, plus you get the actual physics of the system and 2.5D for free. I might do this at some point.

    I know. They're actually so well-known, it's trivial to implement both positional and tangent sampling, but I hate the spline part of the set up, and also Beziers without editor aren't really that useful. All of this is finnicky and I wanted to work with the heart of it, instead of spending the following day rooting out stupid bugs. Now it's easier to switch to something proper, once you know it works. Like you did.

    I guess you can navigate the most of it, because it's similar to what I did before (here and in the crazy tutorial where dude almost died). The only unusual parts are: SimpleSceneViewPanel (which you can find here), the generic array methods (I'm sure you've noticed them), and the rectangle drawers. drawRect2 is the very last thing I did, and so the actual dimensions are a little bit unrefined and need more work, but I had some fun making this.


    Drawing Rectangles
    Rectangles and boxes are always interesting for me, like mini puzzles, because they are incredibly hard to tame in code. So every time I'm doing them I try to figure out a new approach. Here I had a simple spec: I wanted to stick a rectangle to the line skeleton, so that it follows it around. The signature was supposed to be (point, direction, size) and this would work, but then I thought it would be nice to complicate this further by also doing (point1, point2, angle) where the goal was to have the smallest possible rectangle that contains the two points, whose edges are angled by a set amount.

    Here's what happens with two fixed points, when the angle is changing. upload_2023-6-11_17-17-59.png
    So the core idea for drawRect is this:
    Code (csharp):
    1. Rect encaps(Vector2 a, Vector2 b) {
    2.   Vector2 min = leastOf(a, b), max = mostOf(a, b);
    3.   return new Rect(min, max - min);
    4. }
    This produces the smallest rectangle that contains the two points. The only issue is that this is in axis-aligned space. I was lazy to find my 2D rotation which is more suitable here, so I repurposed a quaternion.

    There are two parts to make this work. I transform the points to the axis-aligned space (unrotate), run encaps(ulate) to find Rect, then reuse the drawCircle algorithm for the 4 points, but when I compute them I bring them back to world space (rotate).

    That way I don't bother with the rotation at all, I just take the four angles expressed as
    i * PI / 2
    , but this would get you a rectangle with corners at 0° 90° 180° 270° and you want the corners shifted by +45° so it's
    ((float)i + .5f) * PI / 2
    instead. Then I produce a cartesian coordinate with
    polar
    . Finally, if I set the point at some distance I get a square, but this square is too large and uniform. It's too large because we're sampling a diagonal, which is at √2 distance, and to fix uniformity, I scale X by width and Y by height. Finally this distance needs to be halved, because we're doing this from the center to exploit symmetries. I offset the whole thing to match the center of the rectangle, and then rotate the whole thing.
    Code (csharp):
    1. var p = q * mad(SQRT2_2 * r.size, polar(((float)i + .5f) * PI_2), r.center);
    Yes this doesn't rotate around the local center, it rotates around the world origin. It's a bug haha! But it's canceled out by unrotating around the world origin at the beginning, which is just crazy. In other words, this only works properly if you're close to the world origin, so I have to fix this asap.

    Here's how it should work instead. Before "unrotating"
    Code (csharp):
    1. a = iq * a;
    2. b = iq * b;
    We introduce a common center c and offset by that.
    Code (csharp):
    1. c = (a + b) * .5f;
    2. a = iq * (a - c);
    3. b = iq * (b - c);
    Let's change
    mad
    to allow the last argument to be optional
    Code (csharp):
    1. static Vector2 mad(Vector2 a, Vector2 b, Vector2 c = default) => new Vector2(a.x * b.x + c.x, a.y * b.y + c.y);
    Sounds dumb, but it stands for "multiply-add" -- Unity.Mathematics has a whole family of these because they can be SIMD optimized; this is a pretty frequent operation and it's better to keep the math compatible, so you can easily migrate if you want to.
    And then
    Code (csharp):
    1. var p = c + v2(q * mad(SQRT2_2 * r.size, polar(((float)i + .5f) * PI_2)));
    Now I rotate the local point first, and then offset the whole thing to match the center of the rectangle.
    Phew!

    Code (csharp):
    1. void drawRect2(Vector2 c, Vector2 dir, Vector2 size, Color? color = null, float thickness = 1f) {
    2.   var perp = new Vector2(-dir.y, dir.x);
    3.   var extent = (-size.x * perp + size.y * dir) * .5f;
    4.   drawRect(c + extent, c - extent, -ang(dir), color, thickness);
    5. }
    Here I simply repurpose drawRect. This takes
    dir
    argument (a unit direction) and treats it as a local Y axis. To find the local X axis I do
    var perp = new Vector2(-dir.y, dir.x)
    . Which is incorrect but works thanks to mirror symmetry. Should be
    var perp = new Vector2(dir.y, -dir.x)
    :). Now I can compute the two points by doing
    c + extent
    and
    c - extent
    , where extent is a rectangular half-offset aligned in such a way so that the direction is always up.

    We can also fix some things, for example, the angle being negative here is a mistake I made (axis was
    Vector3.back
    instead of
    Vector3.forward
    ). In fact, we can combine the two ideas, and get rid of quaternions altogether, so let's do rotation in 2D.
    Code (csharp):
    1. static Vector2 rotate(Vector2 point, float rad, Vector2 center = default) {
    2.   point -= center;
    3.   var trig = polar(rad);
    4.   return center + apply_rot(ref point, ref trig);
    5.  
    6.   static Vector2 apply_rot(ref Vector2 p, ref Vector2 trig)
    7.     => new Vector2(
    8.          trig.x * p.x + trig.y * p.y,
    9.          trig.y * p.x - trig.x * p.y
    10.        );
    11. }
    Some tidying up, and now drawRect2 can be expressed like this
    Code (csharp):
    1. void drawRect2(Vector2 c, Vector2 yDir, Vector2 size, Color? color = null, float thickness = 1f) {
    2.   const float SQRT2_2 = 0.70710678f;
    3.   const float PI_2 = MathF.PI / 2f;
    4.  
    5.   if(color.HasValue) Handles.color = color.Value;
    6.   var xDir = new Vector2(yDir.y, -yDir.x);
    7.   var extent = (-size.x * xDir + size.y * yDir) * .5f;
    8.  
    9.   var rot = ang(xDir);
    10.   var r = encaps(rotate(extent, rot, c), rotate(-extent, rot, c));
    11.  
    12.   var last = Vector2.zero;
    13.   for(int i = 0; i <= 4; i++) {
    14.     var l = mad(SQRT2_2 * r.size, polar(((float)i + .5f) * PI_2));
    15.     var p = c + rotate(l, rot);
    16.     if(i > 0) drawSeg(last, p, thickness);
    17.     last = p;
    18.   }
    19. }
    Finally, we can extract the bottom portion that deals with the
    Rect
    alone and produce the following
    Code (csharp):
    1. void drawRect(Rect rect, float rot, Color? color = null, float thickness = 1f) {
    2.   const float SQRT2_2 = 0.70710678f;
    3.   const float PI_2 = MathF.PI / 2f;
    4.  
    5.   if(color.HasValue) Handles.color = color.Value;
    6.  
    7.   var last = Vector2.zero;
    8.   for(int i = 0; i <= 4; i++) {
    9.     var l = mad(SQRT2_2 * rect.size, polar(((float)i + .5f) * PI_2));
    10.     var p = rect.center + rotate(l, rot);
    11.     if(i > 0) drawSeg(last, p, thickness: thickness);
    12.     last = p;
    13.   }
    14. }
    This now looks like a special drawCircle function, and the two variants are then simply
    Code (csharp):
    1. void drawRect(Vector2 a, Vector2 b, float rot, Color? color = null, float thickness = 1f) {
    2.   var c = (a + b) * .5f;
    3.   var rect = encaps(rotate(a, rot, c), rotate(b, rot, c));
    4.   drawRect(rect, rot, color, thickness);
    5. }
    6.  
    7. void drawRect2(Vector2 c, Vector2 yDir, Vector2 size, Color? color = null, float thickness = 1f) {
    8.   var xDir = new Vector2(yDir.y, -yDir.x);
    9.   var extent = (-size.x * xDir + size.y * yDir) * .5f;
    10.   var rot = ang(xDir);
    11.   var rect = encaps(rotate(extent, rot), rotate(-extent, rot));
    12.   rect.center = c;
    13.   drawRect(rect, rot, color, thickness);
    14. }
    Which is an effective way of tying everything down to the essence of this geometry, and makes it possible to observe the two parametrizations in isolation.


    Generic Array Methods
    Here the idea was to -- instead of relying on just a single value -- keep track of a series of historical values and then compute a rolling average on the spot. But I anticipated this would be harder to maintain, so I wanted to set the amount of elements just once, and have it be smart about it and not bother me any more.

    So at the fundamental level, instead of assigning a value to a class var, you store it at the array's last index. But before doing that, the previous values should roll back by one (the oldest one is lost), so effectively it's a FIFO structure, like a conveyor belt. On getting the value back, an average is computed (by summing everything up and dividing by element count). So for example instead of doing
    Code (csharp):
    1. Vector2 _last_B; // class member
    2. _last_B = B; // assignment
    3. Debug.Log(_last_B) // reading
    You'd do
    Code (csharp):
    1. Vector2[] _last_B; // class member (defined in OnEnable)
    2. addToSeries(_last_B, B); // assignment
    3. Debug.Log(avg(_last_B)); // get back the average
    Where addToSeries does the sweeping repositioning and makes use of the newer index range syntax (I actually rarely use this feature).
    Code (csharp):
    1. void addToSeries(Vector2[] array, Vector2 value) {
    2.   for(int i = 1; i < ary.Length; i++) ary[i-1] = ary[i]; // shift-overwrite by 1
    3.   ary[^1] = value; // assign to the last index
    4. }
    And avg does
    Code (csharp):
    1. static Vector2 avg(Vector2[] array, int skip) {
    2.   var sum = Vector2.zero;
    3.   int count = 0;
    4.   for(int i = 0; i < array.Length; i += skip) { sum += array[i]; count++; }
    5.   return sum / (float)count;
    6. }
    (You can also sample every N-th value, I guess for speed.)

    But then I wanted to apply this to mouse coordinates as well, and that's Vector3. And I hate when that happens. The code is exactly the same, but you cannot express such operator-laden code generically. You cannot make any promises to the compiler, because it can't access the actual
    +
    and
    /
    operators from the generic code. This is partly Unity's fault, but mostly C#'s. It is only recently that we got generic math support (Edit: as if that would work in the context of Unity's vectors), but regardless, the common trick is to delegate the unresolvable context to the domain that knows about these intricacies.

    In other words, you can rely on lambdas locally, and resolve them in the caller's concrete domain. In this particular case, it doesn't matter that much, room- or etiquette-wise, but you still get to not repeat the internal implementation.

    So apart from the usual arguments array and skip, here I declare the actual sum and div operations as generic delegates (in pseudo-code)
    Vector2 (Vector2 + Vector2)
    and
    Vector2 (Vector2 / float)
    .

    This is specified as
    Func<Vector2, Vector2, Vector2>
    and
    Func<Vector2, float, Vector2>
    .

    In generic context
    Func<T, T, T>
    and
    Func<T, float, T>
    .
    This is what makes avg transformable
    Code (csharp):
    1. static T avg<T>(T[] ary, int skip, Func<T, T, T> add, Func<T, float, T> div) where T : struct {
    2.   var sum = default(T);
    3.   int count = 0;
    4.   for(int i = 0; i < ary.Length; i += skip) { sum = add(sum, ary[i]); count++; }
    5.   return div(sum, count);
    6. }
    And then
    Code (csharp):
    1. static Vector2 avg(Vector2[] ary, int skip = 1)
    2.   => avg(ary, max(1, skip), (sum, item) => sum + item, (sum, count) => sum / count );
    3.  
    4. static Vector3 avg(Vector3[] ary, int skip = 1)
    5.   => avg(ary, max(1, skip), (sum, item) => sum + item, (sum, count) => sum / count );
    These two look the same, but if you hover over the actual
    +
    and
    /
    operators, you can see they're practically completely different things. Compiler can't possibly know that they ought to be related in any way. Which is a pity but hey.

    On top of this, you're free to experiment with other types of means, i.e. this would be the geometric mean
    Code (csharp):
    1. static Vector2 mean(Vector2[] ary, int skip = 1)
    2.   => mean(ary, max(1, skip), (sum, item) => sum + ln(item), (sum, count) => exp(sum / count) );
    With some provisions in place, such as
    Code (csharp):
    1. static float ln(float n) => MathF.Log(n);
    2. static float exp(float n) => MathF.Exp(n);
    3. static Vector2 ln(Vector2 v) => new Vector2(ln(v.x), ln(v.y));
    4. static Vector2 exp(Vector2 v) => new Vector2(exp(v.x), exp(v.y));

    Kurt's Lerping Trick
    It's this line here
    Code (csharp):
    1. var B = Vector2.Lerp(_last_B[^1], v2(avg(_mpw)), 5f * dt);
    It helps regulate the actual mouse point's overall influence on the math and makes things smoother as a result. In addition to also having the mouse averaged.


    ---
    Everything else should be self-explanatory, or at least I hope so.
    Here's the full corrected version.
    Code (csharp):
    1. using System;
    2. using UnityEngine;
    3. using UnityEditor;
    4. using EditorToolkit;
    5. using static EditorToolkit.SimpleSceneViewPanel;
    6.  
    7. [ExecuteInEditMode]
    8. public class SemiTrailer : MonoBehaviour {
    9.  
    10.   [SerializeField] [Range(0f, 20f)] float _d1 = 4f;
    11.   [SerializeField] [Range(0f, 20f)] float _d2 = 2.5f;
    12.   [SerializeField] [Range(0f, 20f)] float _d3 = 16f;
    13.   [SerializeField] [Range(.01f, 3f)] float _width = 0.6f;
    14.   [SerializeField] [Range(0f, 2f)] float _gizmoScale = 1f;
    15.  
    16.   //-----------------------------------
    17.  
    18.   SimpleSceneViewPanel _panel;
    19.  
    20.   double _start_time;
    21.   double _last_time;
    22.  
    23.   Vector3[] _mpw;
    24.  
    25.   Vector2[] _last_B;
    26.   float _last_angB = 0f;
    27.   float _last_angD = 0f;
    28.   float _saved_psi;
    29.  
    30.   //-----------------------------------
    31.  
    32.   Color _c1 = hexColor(0xedad09);
    33.   Color _c2 = hexColor(0x09bced);
    34.   Color _c3 = hexColor(0x096ced);
    35.   Color _c4 = hexColor(0xad09ed);
    36.  
    37.   //-----------------------------------
    38.  
    39.   void OnEnable() {
    40.     _mpw = new Vector3[12];
    41.     _last_B = new Vector2[24];
    42.     _panel = new SimpleSceneViewPanel("Info", 160f, rows: 4, panelInfo, ScreenCorner.BottomLeft);
    43.     SceneView.duringSceneGui += OnSceneGUI;
    44.     _start_time = Time.realtimeSinceStartupAsDouble;
    45.   }
    46.  
    47.   void OnDisable() {
    48.     destroyPanel();
    49.     SceneView.duringSceneGui -= OnSceneGUI;
    50.   }
    51.  
    52.   //-----------------------------------
    53.  
    54.   void OnDrawGizmos() {
    55.     var time = Time.realtimeSinceStartupAsDouble - _start_time;
    56.     var dt = (float)(time - _last_time);
    57.  
    58.     var B = Vector2.Lerp(_last_B[^1], v2(avg(_mpw)), 5f * dt);
    59.  
    60.     var velB = (B - avg(_last_B)); // / dt;
    61.     var tanB = velB.normalized;
    62.     var angB = ang(tanB);
    63.     var omgB = dang(_last_angB, angB); // / dt;
    64.  
    65.     var A = mad(_d1 + _d2, tanB, B);
    66.  
    67.     var psi = _saved_psi = dang(angB, _last_angD);
    68.  
    69.     var omgD = (velB.magnitude * sin(psi) + _d2 * omgB * cos(psi)) / _d3;
    70.     var angD = moda(_last_angD - omgD); // * dt;
    71.  
    72.     if(float.IsNaN(angD)) angD = angB;
    73.  
    74.     var C = mad(_d2, polar(angB), B);
    75.     var D = mad(_d3, -polar(angD), C);
    76.  
    77.     drawPoint(A, _c1);
    78.     drawSeg(A, C, _c1);
    79.     drawPoint(B, _c2);
    80.     drawSeg(B, C, _c2);
    81.     drawPoint(C, _c3);
    82.     drawSeg(C, D, _c3);
    83.     drawPoint(D, _c4);
    84.  
    85.     var w = (C - A).magnitude * _width;
    86.     drawRect2((A + C) / 2f, (B - A).normalized, new Vector2(w, 2f * w), Color.green, thickness: 1f);
    87.     drawRect2(((C + D) - (C - D).normalized * _d2) / 2f, (C - D).normalized, new Vector2(w, _d3), Color.grey, thickness: 1f);
    88.  
    89.     _last_time = time;
    90.     _last_angB = angB;
    91.     _last_angD = angD;
    92.  
    93.     addToSeries(_last_B, B);
    94.   }
    95.  
    96.   //-----------------------------------
    97.  
    98.   static void addToSeries<T>(T[] ary, T value) where T : struct {
    99.     for(int i = 1; i < ary.Length; i++) ary[i-1] = ary[i];
    100.     ary[^1] = value;
    101.   }
    102.  
    103.   static Vector2 avg(Vector2[] ary, int skip = 1)
    104.     => avg(ary, max(1, skip), (sum, item) => sum + item, (sum, count) => sum / count );
    105.  
    106.   static Vector3 avg(Vector3[] ary, int skip = 1)
    107.     => avg(ary, max(1, skip), (sum, item) => sum + item, (sum, count) => sum / count );
    108.  
    109.   static T avg<T>(T[] ary, int skip, Func<T, T, T> add, Func<T, float, T> div) where T : struct {
    110.     var sum = default(T);
    111.     int count = 0;
    112.     for(int i = 0; i < ary.Length; i += skip) { sum = add(sum, ary[i]); count++; }
    113.     return div(sum, count);
    114.   }
    115.  
    116.   //-----------------------------------
    117.  
    118.   void OnSceneGUI(SceneView view) {
    119.     var e = Event.current;
    120.  
    121.     if(e.isMouse) {
    122.       addToSeries(_mpw, toWorldPoint(e.mousePosition));
    123.       if(_panel.Visible) view.Repaint();
    124.     }
    125.   }
    126.  
    127.   Vector3 toWorldPoint(Vector2 pos) {
    128.     var ray = HandleUtility.GUIPointToWorldRay(pos);
    129.     var plane = new Plane(Vector3.back, Vector3.zero);
    130.     plane.Raycast(ray, out var result);
    131.     return mad(result, ray.direction, ray.origin);
    132.   }
    133.  
    134.   void destroyPanel() {
    135.     if(_panel is not null) {
    136.       _panel.Enabled = false;
    137.       _panel = null;
    138.     }
    139.   }
    140.  
    141.   PanelRow panelInfo(int row)
    142.     => row switch {
    143.          0 => PanelRow.Label($"Mouse: {v2(_mpw[^1]):F3}"),
    144.          1 => PanelRow.Label($"Angle B: {deg(_last_angB):F2}°"),
    145.          2 => PanelRow.Label($"Angle D: {deg(_last_angD):F2}°"),
    146.          3 => PanelRow.Label($"Psi: {deg(_saved_psi):F2}°"),
    147.          _ => null
    148.        };
    149.  
    150.   //-----------------------------------
    151.  
    152.   void drawSeg(Vector2 a, Vector2 b, Color? color = null, float thickness = 2f) {
    153.     if(color.HasValue) Handles.color = color.Value;
    154.     Handles.DrawLine(v3(a), v3(b), thickness * _gizmoScale);
    155.   }
    156.  
    157.   void drawCircle(Vector2 c, float r, int segments = 48, Color? color = null, float thickness = 2f) {
    158.     if(color.HasValue) Handles.color = color.Value;
    159.     var last = Vector2.zero;
    160.     var step = TAU / segments;
    161.     for(int i = 0; i <= segments; i++) {
    162.       var p = mad(r, polar((float)i * step), c);
    163.       if(i > 0) drawSeg(last, p, thickness: thickness);
    164.       last = p;
    165.     }
    166.   }
    167.  
    168.   void drawPoint(Vector2 p, Color? color = null)
    169.     => drawCircle(p, .04f * _gizmoScale, segments: 8, color, thickness: 3f);
    170.  
    171.   void drawRect2(Vector2 c, Vector2 yDir, Vector2 size, Color? color = null, float thickness = 1f) {
    172.     var xDir = new Vector2(yDir.y, -yDir.x);
    173.     var extent = (-size.x * xDir + size.y * yDir) * .5f;
    174.     var rot = ang(xDir);
    175.     var rect = encaps(rotate(extent, rot), rotate(-extent, rot));
    176.     rect.center = c;
    177.     drawRect(rect, rot, color, thickness);
    178.   }
    179.  
    180.   void drawRect(Vector2 a, Vector2 b, float rot, Color? color = null, float thickness = 1f) {
    181.     var c = (a + b) * .5f;
    182.     var rect = encaps(rotate(a, rot, c), rotate(b, rot, c));
    183.     drawRect(rect, rot, color, thickness);
    184.   }
    185.  
    186.   void drawRect(Rect rect, float rot, Color? color = null, float thickness = 1f) {
    187.     const float SQRT2_2 = 0.70710678f;
    188.     const float PI_2 = MathF.PI / 2f;
    189.  
    190.     if(color.HasValue) Handles.color = color.Value;
    191.  
    192.     var last = Vector2.zero;
    193.     for(int i = 0; i <= 4; i++) {
    194.       var l = mad(SQRT2_2 * rect.size, polar(((float)i + .5f) * PI_2));
    195.       var p = rect.center + rotate(l, rot);
    196.       if(i > 0) drawSeg(last, p, thickness: thickness);
    197.       last = p;
    198.     }
    199.   }
    200.  
    201.   Rect encaps(Vector2 a, Vector2 b) {
    202.     Vector2 min = leastOf(a, b), max = mostOf(a, b);
    203.     return new Rect(min, max - min);
    204.   }
    205.  
    206.   //-----------------------------------
    207.  
    208.   // π
    209.   static readonly float PI = MathF.PI;
    210.  
    211.   // one full revolution in radians
    212.   static readonly float TAU = 2f * PI;
    213.  
    214.   // sine
    215.   static float sin(float rad) => MathF.Sin(rad);
    216.  
    217.   // cosine
    218.   static float cos(float rad) => MathF.Cos(rad);
    219.  
    220.   // returns the cartesian coordinates of a polar angle expressed in radians
    221.   static Vector2 polar(float rad) => new Vector2(cos(rad), sin(rad));
    222.  
    223.   // returns an absolute value
    224.   static float abs(float n) => Math.Abs(n);
    225.  
    226.   // returns a square root of some value
    227.   static float sqrt(float n) => MathF.Sqrt(n);
    228.  
    229.   // computes the squared length of a right-angle hypothenuse
    230.   static float sqrhypot(float x, float y) => x * x + y * y;
    231.  
    232.   // multiply-add cmul(Vector2, Vector2) + Vector2
    233.   static Vector2 mad(Vector2 a, Vector2 b, Vector2 c = default) => new Vector2(a.x * b.x + c.x, a.y * b.y + c.y);
    234.  
    235.   // multiply-add float * Vector2 + Vector2
    236.   static Vector2 mad(float a, Vector2 b, Vector2 c) => a * b + c;
    237.  
    238.   // multiply-add float * Vector3 + Vector3
    239.   static Vector3 mad(float a, Vector3 b, Vector3 c) => a * b + c;
    240.  
    241.   // construct a Vector2 from single value
    242.   static Vector2 v2(float n) => new Vector2(n, n);
    243.  
    244.   // convert Vector3 to Vector2 (XY plane)
    245.   static Vector2 v2(Vector3 v) => new Vector2(v.x, v.y);
    246.  
    247.   // convert Vector2 to Vector3 (XY plane)
    248.   static Vector3 v3(Vector2 v) => new Vector3(v.x, v.y, 0f);
    249.  
    250.   // returns the smaller of the two values
    251.   static float min(float a, float b) => MathF.Min(a, b);
    252.  
    253.   // returns the larger of the two values
    254.   static float max(float a, float b) => MathF.Max(a, b);
    255.  
    256.   // returns the larger of the two integers
    257.   static int max(int a, int b) => Math.Max(a, b);
    258.  
    259.   // find the absolute angle of the direction vector
    260.   static float ang(Vector2 v) => MathF.Atan2(v.y, v.x);
    261.  
    262.   // ensures angle modularity in interval [0..TAU)
    263.   static float moda(float a) => (a %= TAU) < 0f? a + TAU : a;
    264.  
    265.   // get signed difference between two angles in interval [0..±PI]
    266.   static float dang(float a, float b) { var d = moda(b - a); return d < PI? d : d - TAU; }
    267.  
    268.   // radians to degrees
    269.   static float deg(float rad) => rad * (180f / PI);
    270.  
    271.   // degrees to radians
    272.   static float rad(float deg) => deg * (PI / 180f);
    273.  
    274.   // constructs a Color32 from lambda expression
    275.   static Color32 c32lambda(Func<int, byte> lambda)
    276.     => new Color32(lambda(0), lambda(1), lambda(2), lambda(3));
    277.  
    278.   // builds Color32 from a 24-bit integer and alpha byte
    279.   static Color32 hexColor(uint value, byte alpha = byte.MaxValue)
    280.     => c32lambda( i => (i < 3)? (byte)( ( value >> ((2-i)<<3) ) & byte.MaxValue )
    281.                               : alpha );
    282.  
    283.   // component-wise minimum
    284.   static Vector2 leastOf(Vector2 a, Vector2 b) => new Vector2(min(a.x, b.x), min(a.y, b.y));
    285.  
    286.   // component-wise maximum
    287.   static Vector2 mostOf(Vector2 a, Vector2 b) => new Vector2(max(a.x, b.x), max(a.y, b.y));
    288.  
    289.   // rotates a point around the center, by angle expressed in radians, in 2D
    290.   static Vector2 rotate(Vector2 point, float rad, Vector2 center = default) {
    291.     point -= center;
    292.     var trig = polar(rad);
    293.     return center + apply_rot(ref point, ref trig);
    294.  
    295.     static Vector2 apply_rot(ref Vector2 p, ref Vector2 trig)
    296.       => new Vector2(
    297.            trig.x * p.x + trig.y * p.y,
    298.            trig.y * p.x - trig.x * p.y
    299.          );
    300.   }
    301.  
    302. }

    Edit: typos
    Edit2: Geometric mean example
    Edit3: Added picture
    Edit4: Fixed a sneaky bug with rotation sign and updated code with proper version
     
    Last edited: Jun 11, 2023
  47. orionsyndrome

    orionsyndrome

    Joined:
    May 4, 2014
    Posts:
    3,318
    Oops I haven't copy/pasted the proper version of the code. Also I tested
    rotate
    for proper handedness and fixed that as well. That was really sneaky. As a result, "unrotating" in drawRect methods now has the same rotation sign as "rotating". This is because rotating the two points counter-rotates the rectangle and vice versa.