c# - Untiy how to add rotation around vector to rotation of object

Question

Video Link to problem

Hi, i'm working on a inverse kinmatics script for a two link chain/arm. It works pretty well. But the only thing i'm missing is the ability to rotate the entire arm around the vector of the base and the target(in blue). This is the script i created:

        using System.Collections;
        using System.Collections.Generic;
        using UnityEngine;
 
        public class IKSolver : MonoBehaviour
     {
    [Header("Joints")]
    public Transform Link0;
    public Transform Link1;
    public Transform Endpoint;
 
    public float angle;
 
    [Header("Target")]
    public Transform Target;
 
    private float length0;
    private float length1;
 
    private void Start()
    {
        length0 = Vector2.Distance(Link0.position, Link1.position);
        length1 = Vector2.Distance(Link1.position, Endpoint.position);
    }
 
    void Update()
    {
        float jointAngle0;
        float jointAngle1;
        float distance = Vector3.Distance(Link0.position, Target.position);
        Vector3 diffVector = Target.position - Link0.position;
 
        float acos = Mathf.Acos((Target.position.y - Link0.position.y) / distance) * Mathf.Rad2Deg;
 
        //calculate how much the arm should rotate around the Y-axis to point towards the EndPoint
        Vector3 root = Link0.rotation.eulerAngles;
        root.y = -Mathf.Atan2(diffVector.z, diffVector.x) * Mathf.Rad2Deg;
        Link0.transform.eulerAngles = root;
 
        //calculate the inverse kinmatics for both links in arm      
        if (length0 + length1 < distance)
        {
            jointAngle0 = acos;
            jointAngle1 = 0f;
        }
        else
        {
            float cosAngle0 = ((distance * distance) + (length0 * length0) - (length1 * length1)) / (2 * distance * length0);
            float angle0 = Mathf.Acos(cosAngle0) * Mathf.Rad2Deg;
            float cosAngle1 = ((length1 * length1) + (length0 * length0) - (distance * distance)) / (2 * length1 * length0);
            float angle1 = Mathf.Acos(cosAngle1) * Mathf.Rad2Deg;
            jointAngle0 = acos - angle0;
            jointAngle1 = 180f - angle1;
        }
 
        //apply the calculated angles to the local transforms of the links    
        Vector3 Euler0 = Link0.transform.localEulerAngles;
        Euler0.z = -jointAngle0;
        Link0.transform.localEulerAngles = Euler0;
        Vector3 Euler1 = Link1.transform.localEulerAngles;
        Euler1.z = -jointAngle1;
        Link1.transform.localEulerAngles = Euler1;
 
        //getting angle that the arm should rotate around the blue vector
        Quaternion angleAxis = Quaternion.AngleAxis(angle, diffVector);
 
        //draw ray between Link0 and endPoint;
        Debug.DrawRay(Link0.position, angleAxis * diffVector, Color.blue);
        //draw ray of the rotation i want to achieve
        Debug.DrawRay(Link0.position, (angleAxis * Quaternion.Euler(root)) * Vector3.up, Color.magenta);
    }
  }

I created this script with the help of Alan Zucconi's blog post. As you can see in the video i can already draw a ray with the angle that i want(in magenta). I would like to rotate the entire arm to this angle. But i can't seem to figure out how to do it.

You can see that i do get the ange with "Quaternion.AngleAxis". and i do apply this to the magenta ray together with the root that i calculated previously.

//getting angle that the arm should rotate around the blue vector
Quaternion angleAxis = Quaternion.AngleAxis(angle, diffVector); 

But whenever i try to apply this rotation + the root rotation to the first link it does not seem to work like the magenta ray. And i cannot seem to figure out how to fix it.

So, like the title of the question states i need to figure out how to add the rotation around the vector diffVector to the already existing rotation root without disturbing the local eulersRotation Euler0.

Any help is appreciated.

question from:https://stackoverflow.com/questions/65857979/untiy-how-to-add-rotation-around-vector-to-rotation-of-object

Answer 1

Waitting for answers