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:
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using UnityEngine.Splines;
public class TrailerMovement : MonoBehaviour
{
public float d1 = 6.2f;
public float d2 = .6f;
public float d3 = 33.4f;
public float deltaTime = 0.5f;
private float time = 0f;
private float angleB = 0f;
private float angleD = 0f;
public SplineContainer testSplineContainer;
GameObject visualizationObjectParent;
public float simulationDuration = 10f;
private void Update()
{
if (Input.GetKeyDown(KeyCode.Space))
{
StartCoroutine(runTrailerSim());
}
}
private IEnumerator runTrailerSim()
{
for (float i = deltaTime; i <= simulationDuration; i+=deltaTime)
{
visualizationObjectParent = new GameObject();
createVisual();
UpdateTrailerPosition();
yield return null;
//yield return new WaitForSeconds(deltaTime);
}
}
void UpdateTrailerPosition()
{
// Calculate position and direction of the rear-wheel center point B
Vector3 positionB = GetPositionB(time);
Vector3 directionB = GetDirectionB(time);
// Calculate position of the front wheel A
Vector3 positionA = positionB + (d1 + d2) * new Vector3(Mathf.Sin(angleB), 0f, Mathf.Cos(angleB));
// Calculate angle between tractor and trailer direction
float anglePsi = angleD - angleB;
// Calculate angular velocity at the center point D of the trailer
float angularVelocityD = Mathf.Abs(GetVelocityB(time)) / d3 * Mathf.Sin(anglePsi) + Mathf.Abs(GetAngularVelocityB(time)) * d2 / d3 * Mathf.Cos(anglePsi);
// Update angles and time
angleD -= angularVelocityD * deltaTime;
angleB = GetDirectionB(time + deltaTime).x; // Assuming x-axis represents the direction
// Calculate position of the trailer wheel's rear center point D
Vector3 positionD = positionB + d2 * new Vector3(Mathf.Sin(angleB), 0f, Mathf.Cos(angleB)) + d3 * new Vector3(-Mathf.Sin(angleD), 0f, -Mathf.Cos(angleD));
// Update time
time += deltaTime;
// Use the calculated position and rotation for your trailer
visualizationObjectParent.transform.rotation = Quaternion.Euler(0f, angleD * Mathf.Rad2Deg, 0f);
visualizationObjectParent.transform.position = positionD;
//transform.position = positionD;
//transform.rotation = Quaternion.Euler(0f, angleD * Mathf.Rad2Deg, 0f); // Assuming rotation is around the y-axis
}
Vector3 GetPositionB(float time)
{
Vector3 position = testSplineContainer.Spline.EvaluatePosition(time / simulationDuration);
visualizationObjectParent.transform.position = position;
return position;
}
Vector3 GetDirectionB(float time)
{
Vector3 tangent = testSplineContainer.Spline.EvaluateTangent(time / simulationDuration);
tangent.Normalize();
return tangent;
}
float GetVelocityB(float time)
{
Vector3 velocity = (GetPositionB(time) - GetPositionB(time - deltaTime)) / deltaTime;
return velocity.magnitude;
}
float GetAngularVelocityB(float time)
{
float directionAtTime = CalculateDirectionB(time);
float directionAfter = CalculateDirectionB(time + deltaTime);
float angularVelocity = (directionAfter - directionAtTime) / deltaTime;
return angularVelocity;
}
float CalculateDirectionB(float time)
{
Vector3 tangent = testSplineContainer.Spline.EvaluateTangent(time / simulationDuration);
float direction = Mathf.Atan2(tangent.z, tangent.x);
if(direction < 0)
{
direction += 2 * Mathf.PI;
}
return direction;
}
private void createVisual()
{
GameObject test = GameObject.CreatePrimitive(PrimitiveType.Cube);
test.transform.localScale = new Vector3(.5f, .5f, 36);
test.transform.parent = visualizationObjectParent.transform;
test.transform.localPosition = new Vector3(0, 0, 18);
test.GetComponent<Renderer>().material.color = Color.red;
}
}
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!
