任意轴C＃

Question

我目前正在研究C＃（WPF）中的3D模型，我正在获取伺服的数据（角度），这将使我的模型在网格中移动。

我试图在z轴上旋转时卡在任意轴上。 当试图旋转时，它实际上在上臂的中心（图像）转动。

有关如何正确实施此建议的任何建议吗？

它应该做什么

它在做什么

我的代码：

/// <summary>
/// Interaction logic for UC_Import_Model.xaml
/// </summary>
public partial class UC_Model_Import : UserControl
{
    /* Importing the base 3D-models */
    ModelImporter importer = new ModelImporter();
    Model3DGroup AxisRobotModel = new Model3DGroup();
    Model3D J0BaseFrame;
    Model3D J1LowerArm;
    Model3D J1LowerArmJoint;
    Model3D J2UpperArm;
    Model3D J3UpperJoint;
    Model3D J4Gripper;
    double x;
    double y;
    double z;


    //the small box to find pints in the 3D World
    BoxVisual3D mybox;

    #region Helper_Box_properties
    public double boxheigth
    {
        get { return mybox.Height; }
        set { mybox.Height = value; }
    }

    public double boxwidth
    {
        get { return mybox.Width; }
        set { mybox.Width = value; }
    }

    public double boxlength
    {
        get { return mybox.Length; }
        set { mybox.Length = value; }
    }


    public double boxX
    {
        get { return mybox.Center.X; }
        set { mybox.Center = new Point3D(value, mybox.Center.Y, mybox.Center.Z); }
    }


    public double boxY
    {
        get { return mybox.Center.Y; }
        set { mybox.Center = new Point3D(mybox.Center.X, value, mybox.Center.Z); }
    }

    public double boxZ
    {
        get { return mybox.Center.Z; }
        set { mybox.Center = new Point3D(mybox.Center.X, mybox.Center.Y, value); }
    }

    #endregion

    public UC_Model_Import()
    {
        InitializeComponent();


        Material material = new DiffuseMaterial(new SolidColorBrush(Colors.Black));
        importer.DefaultMaterial = material;

        /*  Supported Files:
           3D Studio files (.3ds)
           Lightwave files (.lwo)
           Wavefront files (.obj)
           StereoLithography files (.stl)  
        */
        try
        {
            J0BaseFrame = importer.Load("D:\\_Afstudeerstage\\_VS2015-Projecten\\Robot\\RobotControl\\Models\\PartsAxisRobot\\J0BaseFrame.obj");

            material = new DiffuseMaterial(new SolidColorBrush(Colors.OrangeRed));
            importer.DefaultMaterial = material;

            J1LowerArm = importer.Load("D:\\_Afstudeerstage\\_VS2015-Projecten\\Robot\\RobotControl\\Models\\PartsAxisRobot\\J1LowerArm.obj");
            J1LowerArmJoint = importer.Load("D:\\_Afstudeerstage\\_VS2015-Projecten\\Robot\\RobotControl\\Models\\PartsAxisRobot\\J1LowerArmJoint.obj");
            J2UpperArm = importer.Load("D:\\_Afstudeerstage\\_VS2015-Projecten\\Robot\\RobotControl\\Models\\PartsAxisRobot\\J2UpperArm.obj");
            J3UpperJoint = importer.Load("D:\\_Afstudeerstage\\_VS2015-Projecten\\Robot\\RobotControl\\Models\\PartsAxisRobot\\J3UpperJoint.obj");
            J4Gripper = importer.Load("D:\\_Afstudeerstage\\_VS2015-Projecten\\Robot\\RobotControl\\Models\\PartsAxisRobot\\J4Gripper.obj");
        }
        catch (Exception e) { Console.WriteLine(e); }

        AxisRobotModel.Children.Add(J0BaseFrame);
        AxisRobotModel.Children.Add(J1LowerArm);
        AxisRobotModel.Children.Add(J1LowerArmJoint);
        AxisRobotModel.Children.Add(J2UpperArm);
        AxisRobotModel.Children.Add(J3UpperJoint);
        AxisRobotModel.Children.Add(J4Gripper);

        this.AxisModel = AxisRobotModel;

        myModel.Content = this.AxisModel;

        mybox = new BoxVisual3D();
        mybox.Height = 5;
        mybox.Width = 5;
        mybox.Length = 5;
        myModel.Children.Add(mybox);

        overall_grid.DataContext = this;
    }

    #region Getters/setters
    //property for the J1LowerArm movement
    double m_J1LowerArm_angle;
    public double J1LowerArm_angle
    {
        get { return m_J1LowerArm_angle; }
        set
        {
            m_J1LowerArm_angle = value;
        }

    }
    //property for the J1LowerArmJoint movement
    double m_J1LowerArmJoint_angle;
    public double J1LowerArmJoint_angle
    {
        get { return m_J1LowerArmJoint_angle; }
        set
        {
            move_J1LowerArmJoint(new Vector3D(x, y, z),value);
            m_J1LowerArmJoint_angle = value;
        }
    }

    //property for the J2Upperarm movement
    double m_J2UpperArm_angle;
    public double J2UpperArm_angle
    {
        get { return m_J2UpperArm_angle; }
        set
        {
            move_J2UpperArm(new Vector3D(x, y, z), value);
            m_J2UpperArm_angle = value;
        }
    }

    //property for the J3 UpperJoint movement
    double m_J3UpperJoint_angle;
    public double J3UpperJoint_angle
    {
        get { return m_J3UpperJoint_angle; }
        set
        {
            m_J3UpperJoint_angle = value;
        }
    }


    //property for the Gripper movement
    double m_Gripper_angle;
    public double Gripper_angle
    {
        get { return m_Gripper_angle; }
        set
        {
            move_Gripper(new Vector3D(x, y, z), value);
            m_Gripper_angle = value;
        }
    }

    public Model3D AxisModel
    {
        get;
        set;
    }
    #endregion

    void move_J1LowerArm(Vector3D axis, double angle)
    {
        m_J1LowerArm_angle = angle;
        //rotate the object by "angle", the vector describes the axis
        RotateTransform3D J1LowerArm_transform = new RotateTransform3D(new AxisAngleRotation3D(axis, angle));

        //tells where the point of rotation is
        J1LowerArm_transform.CenterX = 0;
        J1LowerArm_transform.CenterY = 0;
        J1LowerArm_transform.CenterZ = 0;

        //apply transformation
        J1LowerArm.Transform = J1LowerArm_transform;

        move_J1LowerArmJoint(axis, 0);
    }

    void move_J1LowerArmJoint(Vector3D axis, double angle)
    {

        //new group of transformations, the group will "add" movements
        var Group_3D = new Transform3DGroup();
        Group_3D.Children.Add(J1LowerArm.Transform);

        //we need to find out where our old point is now
        Point3D origin = Group_3D.Transform(new Point3D(325, 785, 210));

        //rotate the object by "angle", the vector describes the axis
        RotateTransform3D J1LowerArmJoint_transform = new RotateTransform3D(new AxisAngleRotation3D(axis, angle));

        //tells where the point of rotation is
        J1LowerArmJoint_transform.CenterX = origin.X;
        J1LowerArmJoint_transform.CenterY = origin.Y;
        J1LowerArmJoint_transform.CenterZ = origin.Z;

        Group_3D.Children.Add(J1LowerArmJoint_transform);
        J1LowerArmJoint.Transform = J1LowerArmJoint_transform;
        //Move remaining components
        J1LowerArmJoint.Transform = Group_3D;

        move_J2UpperArm(axis, 0);
    }

    void move_J2UpperArm(Vector3D axis, double angle)
    {
        //new group of transformations, the group will "add" movements
        var Group_3D = new Transform3DGroup();
        Group_3D.Children.Add(J1LowerArmJoint.Transform);

        //we need to find out where our old point is now
        Point3D origin = Group_3D.Transform(new Point3D(1225, 1700, -25));
        RotateTransform3D J2UpperArm_transform = new RotateTransform3D(new AxisAngleRotation3D(axis, angle));

        J2UpperArm_transform.CenterX = origin.X;
        J2UpperArm_transform.CenterY = origin.Y;
        J2UpperArm_transform.CenterZ = origin.Z;

        Group_3D.Children.Add(J2UpperArm_transform);
        J2UpperArm.Transform = J2UpperArm_transform;

        J2UpperArm.Transform = Group_3D;
        //move_J3UpperJoint(axis, 0.0);
    }

    void move_J3UpperJoint(Matrix3D matrix, double angle)
    {
        //new group of transformations, the group will "add" movements
        var Group_3D = new Transform3DGroup();
        Group_3D.Children.Add(J2UpperArm.Transform);

        ////we need to find out where our old point is now
        Point3D origin = Group_3D.Transform(new Point3D(2650, 950, -25));


        matrix.RotateAt(new Quaternion(new Vector3D(Math.Cos(m_J2UpperArm_angle.ToRadians()), Math.Sin(m_J2UpperArm_angle.ToRadians()),0), angle), origin);
        Console.WriteLine(matrix);
        MatrixTransform3D J3UpperJoint_Transform = new MatrixTransform3D(matrix);

        //J3UpperJoint.Transform = J3UpperJoint_Transform;
        J3UpperJoint.Transform = Group_3D;
        Group_3D.Children.Add(J3UpperJoint_Transform);        
    }

    void move_Gripper(Vector3D axis, double angle)
    {
        var Group_3D = new Transform3DGroup();
        //Group_3D.Children.Add(J3UpperJoint.Transform);

        //we need to find out where our old point is now
        Point3D origin = Group_3D.Transform(new Point3D(2650, 950, -40));

        //rotate the object by "angle", the vector describes the axis
        RotateTransform3D Gripper_transform = new RotateTransform3D(new AxisAngleRotation3D(axis, angle));

        //tells where the point of rotation is
        Gripper_transform.CenterX = origin.X;
        Gripper_transform.CenterY = origin.Y;
        Gripper_transform.CenterZ = origin.Z;

        J4Gripper.Transform = Gripper_transform; 

        //apply transformation
        J4Gripper.Transform = Group_3D;
        Group_3D.Children.Add(Gripper_transform);
    }

    private void Slider_ValueChanged(object sender, RoutedPropertyChangedEventArgs<double> e)
    {
        move_J2UpperArm(new Vector3D(Math.Sin(m_J1LowerArm_angle.ToRadians()), 0, Math.Cos(m_J1LowerArm_angle.ToRadians())), Anglevalue.Value); 
    }

    private void UpperArm_ValueChanged(object sender, RoutedPropertyChangedEventArgs<double> e)
    {
        move_Gripper(new Vector3D(Math.Sin(m_J1LowerArm_angle.ToRadians()), 0, Math.Cos(m_J1LowerArm_angle.ToRadians())), UpperArm.Value); 
    }

    private void RotateRobot_ValueChanged(object sender, RoutedPropertyChangedEventArgs<double> e)
    {
        if (e.OldValue > e.NewValue) { move_J1LowerArm(new Vector3D(0, 1, 0), RotateRobot.Value); }
        if (e.OldValue < e.NewValue) { move_J1LowerArm(new Vector3D(0, 1, 0), RotateRobot.Value); }
    }

    private void J3Rotation_ValueChanged(object sender, RoutedPropertyChangedEventArgs<double> e)
    {
        move_J3UpperJoint(CalculateRotationMatrix(1, 0, 0),
            J3Rotation.Value);
    }

    private void J1Rotation_ValueChanged(object sender, RoutedPropertyChangedEventArgs<double> e)
    {
        move_J1LowerArmJoint(new Vector3D(Math.Sin(m_J1LowerArm_angle.ToRadians()), 0, Math.Cos(m_J1LowerArm_angle.ToRadians())), J1Rotation.Value);

    }

    Matrix3D CalculateRotationMatrix(double x, double y, double z)
    {
        Matrix3D matrix = new Matrix3D();

        matrix.Rotate(new Quaternion(new Vector3D(1, 0, 0), x));
        matrix.Rotate(new Quaternion(new Vector3D(0, 1, 0) * matrix, y));
        matrix.Rotate(new Quaternion(new Vector3D(0, 0, 1) * matrix, z));

        return matrix;
    }

    /// <summary>
    /// Convert to Radians.
    /// </summary>
    /// <param name="val">The value to convert to radians</param>
    /// <returns>The value in radians</returns>

}
public static class NumericExtensions
{
    public static double ToRadians(this double val)
    {
        return (Math.PI / 180) * val;
    }
}
}