unity 调用c++ dll同时将unity的值传给c++

最近因为项目的需要，实现了将C++工程生成dll在unity中调用，但同时发现如果能让unity中值实时的传给c++则就完美了，而不只是把c++计算的结果给unity。

通过验证是可行的。

C++ 生成dll导出关键部分

// SPDX-License-Identifier: MIT
// Copyright (c) 2021 Manuel Stoiber, German Aerospace Center (DLR)

//#include <srt3d/azure_kinect_camera.h>
#include <srt3d/my_camera.h>
#include <srt3d/camera.h>
#include <srt3d/body.h>
#include <srt3d/common.h>
#include <srt3d/normal_viewer.h>
#include <srt3d/occlusion_renderer.h>
#include <srt3d/region_modality.h>
#include <srt3d/renderer_geometry.h>
#include <srt3d/tracker.h>
#include <cmath>
#include <Eigen/Geometry>
#include <filesystem>
#include <memory>
#include <string>
#include <Windows.h>

//Generate body1 template view 2562 of 2562

//global variables
const char* modelName = "";

const std::filesystem::path model_directory{ "D:\\sourcecode\\STR3D\\rbotdataset\\" };

constexpr bool kSaveViewerImage = false;
const std::filesystem::path viewer_save_directory{ "D:\\sourcecode\\STR3D\\SRT3D\\images\\" };

 Set up tracker and renderer geometry
auto tracker_ptr{ std::make_shared<srt3d::Tracker>("tracker") };
auto renderer_geometry_ptr{ std::make_shared<srt3d::RendererGeometry>("renderer geometry") };

 Set up camera

auto camera_ptr{ std::make_shared < srt3d::MyCamera>("RGB") };
 Set up viewers

auto viewer_ptr{ std::make_shared<srt3d::NormalViewer>("viewer", camera_ptr, renderer_geometry_ptr) };


 Set up body1 (change accordingly)
//const std::filesystem::path body1_geometry_path{ "D:\\sourcecode\\STR3D\\rbotdataset\\motorpart01.obj" };
const std::filesystem::path body1_geometry_path{ "D:\\sourcecode\\STR3D\\rbotdataset\\duck.obj" };

srt3d::Transform3fA body1_geometry2body_pose{ Eigen::Translation3f(0.0f, 0.0f, 0.0f) };
srt3d::Transform3fA body1_world2body_pose;
srt3d::Transform3fA realtime_myObj2world_pose;


auto body1_ptr{ std::make_shared<srt3d::Body>("body1", body1_geometry_path,
											0.001f, true, false, 0.3f,
											 body1_geometry2body_pose, 1) };

float location[7] = { 0,0, 0, 0,0, 0, 0 };

exe程序
extern "C" __declspec(dllexport)
void main()
{

	body1_world2body_pose.matrix() <<
		1.0f, 0.0f, 0.0f, 0.095f,
		0.0f, 1.0f, 0.0f, 0.1f,
		0.0f, 0.0f, 1.0f, -0.4f,
		0.0f, 0.0f, 0.0f, 1.0f;
	tracker_ptr->AddViewer(viewer_ptr);

	if (kSaveViewerImage) viewer_ptr->StartSavingImages(viewer_save_directory);


	//body1_ptr->set_body2world_pose(body1_world2body_pose);
	body1_ptr->set_world2body_pose(body1_world2body_pose);//原论文code
	renderer_geometry_ptr->AddBody(body1_ptr);


	// Set up model body 1
	//auto body1_model_ptr{ std::make_shared<srt3d::Model>("body1_model", body1_ptr, model_directory, "motorpart01_model.bin") };
	auto body1_model_ptr{ std::make_shared<srt3d::Model>("body1_model", body1_ptr, model_directory, "duck.bin") };

	// Set up region modality body 1
	auto body1_region_modality_ptr{ std::make_shared<srt3d::RegionModality>(
		"body1_region_modality", body1_ptr, body1_model_ptr, camera_ptr) };
	tracker_ptr->AddRegionModality(body1_region_modality_ptr);

	// Set up occlusion renderer
	auto occlusion_renderer_ptr{ std::make_shared<srt3d::OcclusionRenderer>(
		"occlusion_renderer", renderer_geometry_ptr, camera_ptr) };
	body1_region_modality_ptr->UseOcclusionHandling(occlusion_renderer_ptr);

	 Start tracking
	tracker_ptr->SetUpTracker();
	//tracker_ptr->StartTracker(false);
	//float pos_value[3] = {1., 2.8, 2.33};
	//tracker_ptr->StartTracker(false, pos_value);
	//tracker_ptr->StartTracker(false, body1_ptr, &a);
	// 
	if (!tracker_ptr->set_up()) {
		std::cerr << "Set up tracker " << tracker_ptr->set_name() << " first" << std::endl;
		return;
	}
	tracker_ptr->set_start_tracking(false);

}


//将旋转矩阵转化为四元数
Eigen::Quaterniond Matrix2Quaternion(Eigen::Matrix<float, 4, 4> matrixofBody)
{

	Eigen::Matrix<float, 4, 4> matrix_body2world_pose;
	//Eigen::Matrix<float, 3, 3> matrix_rotation;
	Eigen::Matrix3d matrix_rotation;

	//Eigen::Quaterniond matrix_quarternion;

	matrix_body2world_pose = matrixofBody;
	//matrix_body2world_pose = body1_ptr->body2world_pose().matrix();

	//body1_ptr->geometry2world_pose().matrix();
	//for (size_t i = 0; i < 4; i++)
	//{
	//	for (size_t j = 0; j < 4; j++)
	//	{
	//		std::cout << matrix_body2world_pose(i, j) << "\t";
	//	}

	//}

	//取出旋转矩阵
	for (size_t i = 0; i < 3; i++)
	{
		for (size_t j = 0; j < 3; j++)
		{
			matrix_rotation(i, j) = matrix_body2world_pose(i, j);
		}
	}

	//std::cout << "---------------matrix_quarternion-----------------" << std::endl;
	std::cout << matrix_quarternion << std::endl;
	//std::cout << matrix_quarternion.x() << "\t" <<matrix_quarternion.y() << "\t"  
	//	<< matrix_quarternion.z() << "\t" << matrix_quarternion.z() << "\t" << std::endl;

	return Eigen::Quaterniond(matrix_rotation);
}

Eigen::Quaterniond matrix_quarternion;
Eigen::Matrix<float, 4, 4> matrix_body2world_pose;
extern "C" __declspec(dllexport)
int start_track(int iteration, float* pos_array, bool* update, float x, float y, float z, float w, float pos_x, float pos_y, float pos_z)
//int start_track(int iteration, float* pos_array, bool* update, float pos_z)

{
	/// <summary>
/// For unity dll test
/// </summary>
/// <param name="pos_array"></param>
/// <returns></returns>

		if (tracker_ptr->get_start_tracking())
		{
			if (!tracker_ptr->StartRegionModalities())
			{
				return -1;
			}
			tracker_ptr->set_tracking_started(true);
			tracker_ptr->set_start_tracking(false);

			//std::cout << "StartRegionModalities" << std::endl;



		}
		if (tracker_ptr->get_tracking_started())
		{
			//std::cout << "--------------------------------" << std::endl;
			//std::cout << body1_ptr->body2world_pose().matrix() << std::endl;

			//当物体追踪后实时更新将我们的旋转和平移传给unity
			matrix_body2world_pose = body1_ptr->body2world_pose().matrix();
			matrix_quarternion = Matrix2Quaternion(matrix_body2world_pose);


			//右手坐标转化为左手坐标 x,z取反
			保存表示旋转的四元数和平移的值
			float location[7] = { -matrix_quarternion.x(), matrix_quarternion.y(), -matrix_quarternion.z(), matrix_quarternion.w(),
									matrix_body2world_pose(0, 3), -matrix_body2world_pose(1, 3),matrix_body2world_pose(2, 3) };
			memcpy(pos_array, location, sizeof(float) * 7);

			std::cout << "---------------------translation-----------------------------" << std::endl;
			std::cout << matrix_body2world_pose(0, 3) << "\t" << matrix_body2world_pose(1, 3) << "\t" << matrix_body2world_pose(2, 3) << std::endl;
			

			//控制unity3d 进程的退出
			int update_is[2] = { tracker_ptr->get_update(), 0 };
			memcpy(update, update_is, sizeof(int)*2);


			if (!tracker_ptr->ExecuteTrackingCycle(iteration))
			{
				std::cout << "ExecuteTrackingCycle--break" << std::endl;
				int update_is[2] = { tracker_ptr->get_update(), 2 };
				memcpy(update, update_is, sizeof(int) * 2);
				return -1;
			}
		}
		else
		{
			//传unity物体最初的位置
			//程序开始后将初始化的旋转和平移传给unity
			if (!tracker_ptr->get_update())
			{

				matrix_body2world_pose = body1_ptr->body2world_pose().matrix();
				matrix_quarternion = Matrix2Quaternion(matrix_body2world_pose);
				float location[7] = { -matrix_quarternion.x(), matrix_quarternion.y(), -matrix_quarternion.z(), matrix_quarternion.w(),
						  matrix_body2world_pose(0, 3), -matrix_body2world_pose(1, 3),matrix_body2world_pose(2, 3) };
				memcpy(pos_array, location, sizeof(float) * 7);

			}
			else
			{

				
				int update_is[2] = { tracker_ptr->get_update(), 1 };
				memcpy(update, update_is, sizeof(int) * 2);  //控制unity3d 进程的退出

				接收unity中的物体位姿
				Eigen::Quaterniond quat_u3d(w, x, y, z);

				Eigen::Matrix3d rotation_matrix = quat_u3d.matrix();

				for (size_t i = 0; i < 3; i++)
				{
					for (size_t j = 0; j < 3; j++)
					{
						body1_world2body_pose.matrix()(i, j) = rotation_matrix.matrix()(i, j);
					}
				}

				body1_world2body_pose.matrix()(0, 3) = pos_x;
				body1_world2body_pose.matrix()(1, 3) = pos_y;
				body1_world2body_pose.matrix()(2, 3) = pos_z;

				body1_world2body_pose.matrix()(3, 0) = 0;
				body1_world2body_pose.matrix()(3, 1) = 0;
				body1_world2body_pose.matrix()(3, 2) = 0;
				body1_world2body_pose.matrix()(3, 3) = 1;

				body1_ptr->set_body2world_pose(body1_world2body_pose);
			}


			if (!tracker_ptr->ExecuteViewingCycle(iteration))
			{
				//控制unity3d 进程的退出
				int update_is[2] = { tracker_ptr->get_update(), 2 };
				memcpy(update, update_is, sizeof(int)*2);
				return -1;
			}
		}
	

	return 0;
}

set_body2world_pose实现物体绕自身的旋转

 如果用set_world2body_pose则是物体绕世界坐标原点旋转。

void Body::set_body2world_pose(const Transform3fA &body2world_pose) {
  body2world_pose_ = body2world_pose;
  world2body_pose_ = body2world_pose_.inverse();
  geometry2world_pose_ = body2world_pose_ * geometry2body_pose_;
  world2geometry_pose_ = geometry2world_pose_.inverse();
}

void Body::set_world2body_pose(const Transform3fA &world2body_pose) {
  world2body_pose_ = world2body_pose;
  body2world_pose_ = world2body_pose_.inverse();
  geometry2world_pose_ = body2world_pose_ * geometry2body_pose_;
  world2geometry_pose_ = geometry2world_pose_.inverse();
}

void Body::set_geometry2body_pose(const Transform3fA &geometry2body_pose) {
  geometry2body_pose_ = geometry2body_pose;
  geometry2world_pose_ = body2world_pose_ * geometry2body_pose_;
  world2geometry_pose_ = geometry2world_pose_.inverse();
}

const Transform3fA &Body::geometry2body_pose() const {
  return geometry2body_pose_;
}

const Transform3fA &Body::body2world_pose() const { return body2world_pose_; }

const Transform3fA &Body::world2body_pose() const { return world2body_pose_; }

const Transform3fA &Body::geometry2world_pose() const {
  return geometry2world_pose_;
}

const Transform3fA &Body::world2geometry_pose() const {
  return world2geometry_pose_;
}

unity中调用 dll并将值传给c++工程

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using System.Runtime.InteropServices;
using System;
using fts;

public class useDll : MonoBehaviour
{

    [DllImport("run_on_camera_sequence")]
    static extern void main();

    [DllImport("run_on_camera_sequence")]
    //static extern int start_track(int iteration, float[] pos_array, int[] is_update,float[] pos_rt);
    static extern int start_track(int iteration, float[] pos_array, int[] is_update, float x, float y, float z, float w, float pos_x, float pos_y, float pos_z);



    // 为接收dll中计算值开辟空间
    private float[] pos_array = new float[7];
    private int[] is_update = new int[2];

    // 申请变量向dll中传值，改变c++程序的运行逻辑
    // 物体的position
    private float pos_x = 0.095f;
    private float pos_y = 0.1f;
    private float pos_z = -0.4f;

    // 物体的旋转
    private float x = 0.0f;
    private float y = 0.0f;
    private float z = 0.0f;
    private float w = 0.0f;

    Quaternion rotation_ = new Quaternion(0, 0, 0, 1);
    Vector3 trans_ = new Vector3(0, 0, 0);

    public GameObject Target;
    int stop_run = 0;
    bool change_pos = true;

    
    int iteration = 0;


    // Start is called before the first frame update
    void Start()
    {

        main();

        //FooPluginAPI_Auto.mainFun();

    }

    // Update is called once per frame
    void Update()
    {

        if (stop_run != -1)
        {
            //FooPluginAPI_Auto.start_trackFun(iteration, pos_array, is_update);
            stop_run = start_track(iteration, pos_array, is_update, x, y, z, w, pos_x, pos_y, pos_z);
            print(pos_array[4] + "**********" + pos_array[5] + "**********" + pos_array[6] + "**********");
            iteration += 1;

            if (is_update[0] != is_update[1] && (is_update[0] * is_update[1]) == 0)
            {
                rotation_.x = pos_array[0];
                rotation_.y = pos_array[1];
                rotation_.z = pos_array[2];
                rotation_.w = pos_array[3];
                Target.transform.localRotation = rotation_;

                trans_.x = pos_array[4];
                trans_.y = pos_array[5];
                trans_.z = pos_array[6];
                Target.transform.localPosition = trans_;

                print("****localRotation*****" + Target.transform.localRotation + "****localPosition*****" + Target.transform.localPosition);
                print("---change---");
            }

            print("-------[0]------" + is_update[0] + "-------[1]------" + is_update[1]);


            if (is_update[0] == is_update[1])
            {
                x = -Target.transform.localRotation.x;
                y = Target.transform.localRotation.y;
                z = -Target.transform.localRotation.z;
                w = Target.transform.localRotation.w;

                pos_x = Target.transform.localPosition.x;
                pos_y = -Target.transform.localPosition.y;
                pos_z = Target.transform.localPosition.z;

                print("***********pos_z************" + pos_z + "******is_update[0]*****" + is_update[0] + "******is_update[1]*****" + is_update[1]);

                //pos_z = Target.transform.localPosition.z;
            }

        }
        //if ( is_update[1] == 2)
        //{
        //    //is_update[0] = -1;
        //    //is_update[1] = 0;

        //    print("free room");

        //}

    }


}

float[] pos_array, int[] is_update

这两个变量是在unity中使用c++的计算的结果

int iteration,float x, float y, float z, float w, float pos_x, float pos_y, float pos_z

这八个值是从unity中赋值传给c++ dll让其控制程序的逻辑。

直接传数组一下就过去了。