API:Vector3 Vector2=vector1.normalized;

本文通过Unity引擎代码展示了如何将向量规范化,并使用Debug.DrawLine方法进行视觉展示。介绍了两种向量规范化的方法:一是通过除以向量的大小;二是使用Unity内置的normalized方法。代码实例有助于理解向量规范化在游戏开发中的应用。

代码实现:

//方向
void Demo02(http://www.amjmh.com/v/BIBRGZ_558768/)
{
Vector3 pos = this.transform.position;
Debug.DrawLine(Vector3.zero, this.transform.position);
//数学公式
Vector3 dir1 = pos / pos.magnitude;
//UnityAPI 封装好的方法
Vector3 dir2 = pos.normalized;
Debug.DrawLine(Vector3.zero,dir1,Color.red);
}
--------------------- 

转载于:https://www.cnblogs.com/hyhy904/p/11304683.html

test.cpp:86:42: error: conversion from ‘Eigen::Matrix3d’ {aka ‘Eigen::Matrix<double, 3, 3>’} to non-scalar type ‘std::vector<std::vector<double> >’ requested 86 | std::vector<std::vector<double>> A = R; | ^ test.cpp:88:42: error: conversion from ‘Eigen::Matrix3f’ {aka ‘Eigen::Matrix<float, 3, 3>’} to non-scalar type ‘std::vector<std::vector<double> >’ requested 88 | std::vector<std::vector<double>> B = RR; | ^~ test.cpp:105:38: error: conflicting declaration ‘std::vector<std::vector<double> > R’ 105 | std::vector<std::vector<double>> R(3, std::vector<double>(3, 0.0)); | ^ test.cpp:32:14: note: previous declaration as ‘Eigen::Matrix3d R’ 32 | Matrix3d R; | ^ test.cpp:109:21: error: invalid types ‘Eigen::DenseCoeffsBase<Eigen::Matrix<double, 3, 3>, 1>::Scalar {aka double}[int]’ for array subscript 109 | R[i][j] += C[i][k] * temp[k][j]; | ^ test.cpp:118:45: error: ‘setprecision’ is not a member of ‘std’ 118 | std::cout << std::fixed << std::setprecision(6) | ^~~~~~~~~~~~ test.cpp:119:31: error: ‘setw’ is not a member of ‘std’ 119 | << std::setw(12) << R[i][j]; | ^~~~ test.cpp:119:47: error: invalid types ‘Eigen::DenseCoeffsBase<Eigen::Matrix<double, 3, 3>, 1>::Scalar {aka double}[int]’ for array subscript 119 | << std::setw(12) << R[i][j]; | ^ test.cpp:125:22: error: invalid types ‘Eigen::DenseCoeffsBase<Eigen::Matrix<double, 3, 3>, 1>::Scalar {aka double}[int]’ for array subscript 125 | double r11 = R[0][0], r12 = R[0][1], r13 = R[0][2]; | ^ test.cpp:126:22: error: invalid types ‘Eigen::DenseCoeffsBase<Eigen::Matrix<double, 3, 3>, 1>::Scalar {aka double}[int]’ for array subscript 126 | double r21 = R[1][0], r22 = R[1][1], r23 = R[1][2]; | ^ test.cpp:127:22: error: invalid types ‘Eigen::DenseCoeffsBase<Eigen::Matrix<double, 3, 3>, 1>::Scalar {aka double}[int]’ for array subscript 127 | double r31 = R[2][0], r32 = R[2][1], r33 = R[2][2]; | ^ test.cpp:130:12: error: redeclaration of ‘double pitch’ 130 | double pitch = std::atan2(-r31, std::sqrt(r32*r32 + r33*r33)); | ^~~~~ test.cpp:36:17: note: ‘double pitch’ previously declared here 36 | double yaw, pitch, roll; | ^~~~~ test.cpp:130:47: error: ‘r32’ was not declared in this scope; did you mean ‘r31’? 130 | double pitch = std::atan2(-r31, std::sqrt(r32*r32 + r33*r33)); | ^~~ | r31 test.cpp:130:57: error: ‘r33’ was not declared in this scope; did you mean ‘r31’? 130 | double pitch = std::atan2(-r31, std::sqrt(r32*r32 + r33*r33)); | ^~~ | r31 test.cpp:133:12: error: redeclaration of ‘double yaw’ 133 | double yaw = std::atan2(r21, r11); | ^~~ test.cpp:36:12: note: ‘double yaw’ previously declared here 36 | double yaw, pitch, roll; | ^~~ test.cpp:136:12: error: redeclaration of ‘double roll’ 136 | double roll = std::atan2(r32, r33); | ^~~~ test.cpp:36:24: note: ‘double roll’ previously declared here 36 | double yaw, pitch, roll; | ^~~~ In file included from /usr/include/eigen3/Eigen/Core:164, from /usr/include/eigen3/Eigen/Dense:1, from test.cpp:6: /usr/include/eigen3/Eigen/src/Core/DenseCoeffsBase.h: In instantiation of ‘Eigen::DenseCoeffsBase<Derived, 1>::Scalar& Eigen::DenseCoeffsBase<Derived, 1>::operator[](Eigen::Index) [with Derived = Eigen::Matrix<double, 3, 3>; Eigen::DenseCoeffsBase<Derived, 1>::Scalar = double; Eigen::Index = long int]’: test.cpp:109:20: required from here /usr/include/eigen3/Eigen/src/Core/DenseCoeffsBase.h:408:36: error: static assertion failed: THE_BRACKET_OPERATOR_IS_ONLY_FOR_VECTORS__USE_THE_PARENTHESIS_OPERATOR_INSTEAD 408 | EIGEN_STATIC_ASSERT(Derived::IsVectorAtCompileTime, | ^~~~~~~~~~~~~~~~~~~~~ /usr/include/eigen3/Eigen/src/Core/util/StaticAssert.h:33:54: note: in definition of macro ‘EIGEN_STATIC_ASSERT’ 33 | #define EIGEN_STATIC_ASSERT(X,MSG) static_assert(X,#MSG); | ^ /usr/include/eigen3/Eigen/src/Core/DenseCoeffsBase.h:408:36: note: ‘Eigen::Matrix<double, 3, 3>::IsVectorAtCompileTime’ evaluates to false 408 | EIGEN_STATIC_ASSERT(Derived::IsVectorAtCompileTime, | ^~~~~~~~~~~~~~~~~~~~~ /usr/include/eigen3/Eigen/src/Core/util/StaticAssert.h:33:54: note: in definition of macro ‘EIGEN_STATIC_ASSERT’ 33 | #define EIGEN_STATIC_ASSERT(X,MSG) static_assert(X,#MSG); // g++ -std=c++11 -I/usr/include/eigen3 test.cpp -o rotation_matrix #include <iostream> #include <cmath> #include <Eigen/Dense> #include <vector> using namespace Eigen; using namespace std; int main() { // 输入向量 Vector3d znew(-0.0741472, 0.321715, -0.943929); Vector3d xnew(-0.997053, 0.00682574, 0.0764145); // 1. 归一化向量 Vector3d z_unit = znew.normalized(); Vector3d x_unit = xnew.normalized(); // 2. Gram-Schmidt正交化 // 移除x_unit在z_unit方向的分量 double dot_product = x_unit.dot(z_unit); Vector3d x_ortho = x_unit - dot_product * z_unit; x_ortho.normalize(); // 3. 计算正交的y轴 Vector3d y_unit = z_unit.cross(x_ortho); y_unit.normalize(); // 4. 构建旋转矩阵 Matrix3d R; R << x_ortho, y_unit, z_unit; // 5. 计算欧拉角 (Z-Y-X顺序: yaw-pitch-roll) double yaw, pitch, roll; // 计算俯仰角 (pitch) pitch = std::asin(-R(2, 0)); // -r31 // 处理万向锁情况 (cos(pitch) ≈ 0) if (std::abs(std::cos(pitch)) > 1e-6) { yaw = std::atan2(R(1, 0), R(0, 0)); // r21, r11 roll = std::atan2(R(2, 1), R(2, 2)); // r32, r33 } else { // 万向锁时使用备用公式 yaw = std::atan2(-R(0, 1), R(1, 1)); roll = 0.0; // 横滚角自由度丢失,设为0 } // 弧度转角度 const double rad2deg = 180.0 / M_PI; yaw *= rad2deg; pitch *= rad2deg; roll *= rad2deg; // 规范化角度到[0, 360)范围 auto normalize_angle = [](double angle) { angle = std::fmod(angle, 360.0); if (angle < 0) angle += 360.0; return angle; }; yaw = normalize_angle(yaw); roll = normalize_angle(roll); // 输出结果 std::cout << "Rotation Matrix R:\n" << R << "\n\n"; std::cout << "Euler Angles (Z-Y-X order, degrees):\n"; std::cout << "Yaw (ψ): " << yaw << "°\n"; std::cout << "Pitch (θ): " << pitch << "°\n"; std::cout << "Roll (φ): " << roll << "°\n"; // 定义欧拉角 (Z-Y-X 顺序) const float yaw2 = M_PI / 2; // 绕 Z 轴 const float pitch2 = 0.0f; // 绕 Y 轴 const float roll2 = -M_PI / 2; // 绕 X 轴 // 构建旋转矩阵 (Z-Y-X 欧拉角) Eigen::Matrix3f RR; RR = Eigen::AngleAxisf(yaw2, Eigen::Vector3f::UnitZ()) * Eigen::AngleAxisf(pitch2, Eigen::Vector3f::UnitY()) * Eigen::AngleAxisf(roll2, Eigen::Vector3f::UnitX()); std::cout << "Rotation Matrix RR:\n" << RR << std::endl; std::vector<std::vector<double>> A = R; std::vector<std::vector<double>> B = RR; std::vector<std::vector<double>> C = {{-0.99595, 0.0633803, 0.0637694}, {-0.0621051, -0.997832, 0.0217865}, {0.065012, 0.0177379, 0.997727}}; // 计算 B * A std::vector<std::vector<double>> temp(3, std::vector<double>(3, 0.0)); for (int i = 0; i < 3; ++i) { for (int j = 0; j < 3; ++j) { for (int k = 0; k < 3; ++k) { temp[i][j] += B[i][k] * A[k][j]; } } } // 计算 C * temp std::vector<std::vector<double>> R(3, std::vector<double>(3, 0.0)); for (int i = 0; i < 3; ++i) { for (int j = 0; j < 3; ++j) { for (int k = 0; k < 3; ++k) { R[i][j] += C[i][k] * temp[k][j]; } } } // 输出合矩阵 std::cout << "Combined Matrix:" << std::endl; for (int i = 0; i < 3; ++i) { for (int j = 0; j < 3; ++j) { std::cout << std::fixed << std::setprecision(6) << std::setw(12) << R[i][j]; } std::cout << std::endl; } // 提取欧拉角 (ZYX顺序) double r11 = R[0][0], r12 = R[0][1], r13 = R[0][2]; double r21 = R[1][0], r22 = R[1][1], r23 = R[1][2]; double r31 = R[2][0], r32 = R[2][1], r33 = R[2][2]; // 计算pitch (Y轴) double pitch = std::atan2(-r31, std::sqrt(r32*r32 + r33*r33)); // 计算yaw (Z轴) double yaw = std::atan2(r21, r11); // 计算roll (X轴) double roll = std::atan2(r32, r33); // 转换为角度并取反 double yaw_deg = -yaw * 180.0 / M_PI; // 取反 double pitch_deg = -pitch * 180.0 / M_PI; // 取反 double roll_deg = -roll * 180.0 / M_PI; // 取反 // 角度归一化 (-180°到180°) yaw_deg = std::fmod(yaw_deg + 180.0, 360.0) - 180.0; pitch_deg = std::fmod(pitch_deg + 180.0, 360.0) - 180.0; roll_deg = std::fmod(roll_deg + 180.0, 360.0) - 180.0; // 输出修正后的欧拉角 std::cout << "\nCorrected Euler Angles (ZYX order):" << std::endl; std::cout << "Yaw (Z) : " << yaw_deg << "°" << std::endl; std::cout << "Pitch (Y) : " << pitch_deg << "°" << std::endl; std::cout << "Roll (X) : " << roll_deg << "°" << std::endl; return 0; }请修正语法
08-16
#include "CylinderFitter.h" #include "CircleFitter.h" #include <corecrt_math_defines.h> #include <Eigen/Dense> #include<iostream> namespace Gauss { double F(Fitting::Cylinder cylinder, const Point& p) { double di = (p - cylinder.center).cross(cylinder.orient).norm() - cylinder.r; return di; } double GetError(Fitting::Cylinder cylinder, const std::vector<Eigen::Vector3d>& points) { double err = 0; for (auto& p : points) { double d = F(cylinder, p); err += d * d; } err /= points.size(); return err; } Fitting::Matrix CylinderFitter::Jacobi(const std::vector<Eigen::Vector3d>& points) { int n = points.size(); Fitting::Matrix J(n, 5); for (int i = 0; i < n; ++i) { auto& p = points[i]; double ri = Eigen::Vector2d(p.x(), p.y()).norm(); //di求导 J(i, 0) = -p.x() / ri; J(i, 1) = -p.y() / ri; J(i, 2) = -p.x() * p.z() / ri; J(i, 3) = -p.y() * p.z() / ri; J(i, 4) = -1; } return J; } void CylinderFitter::beforHook(const std::vector<Eigen::Vector3d>& points) { U = Fitting::getRotationByOrient(cylinder.orient); for (int i = 0; i < points.size(); ++i) { transPoints[i] = U * (points[i] - cylinder.center); } } void CylinderFitter::afterHook(const Eigen::VectorXd& xp) { cylinder.center += U.transpose() * Eigen::Vector3d(xp(0), xp(1), -xp(2)*xp(0)-xp(3)*xp(1)); cylinder.orient = U.transpose() * Eigen::Vector3d(xp(2), xp(3), 1).normalized(); cylinder.r += xp(4); } Eigen::VectorXd CylinderFitter::getDArray(const std::vector<Eigen::Vector3d>& points) { int n = points.size(); Eigen::VectorXd D(points.size()); for (int i = 0; i < points.size(); ++i) D(i) = Eigen::Vector2d(points[i].x(), points[i].y()).norm() - cylinder.r; return D; } bool CylinderFitter::GetInitFit(const std::vector<Eigen::Vector3d>& points) { if (points.size() < 5)return false; double cylinerErr = -1; transPoints.resize(points.size()); Point center = Fitting::getCenter(points); // 拟合平面 Fitting::FittingBase* fb = new CircleFitter(); Fitting::Circle cir; int cnt = 0; for (double i = 0; i < 180; ++i) { double theta = i / 180 * M_PI; for (double j = 0; j < 180; ++j) { double alpha = j / 180 * M_PI; Point orient(cos(theta) , sin(theta)*cos(alpha),sin(theta)*sin(alpha)); // 投影平面 for (int k = 0; k < points.size(); ++k) { double d = (center-points[k]).dot(orient); transPoints[k] = points[k] + d * orient; } // 拟合圆 double err = fb->Fitting(transPoints, &cir); if (err>0 && (cylinerErr < 0 || err < cylinerErr)) { //std::cout << "error : "<< err << std::endl; cylinerErr = err; cylinder.center = cir.center; cylinder.orient = cir.orient; cylinder.r = cir.r; } //cnt++; //if(cnt%100==0) std::cout << cnt << std::endl; } if (cylinerErr < 1e-4)break; } delete fb; return true; } double CylinderFitter::F(const Eigen::Vector3d& p) { return Gauss::F(cylinder, p); } double CylinderFitter::GetError(const std::vector<Eigen::Vector3d>& points) { return Gauss::GetError(cylinder, points); } void CylinderFitter::Copy(void* ele) { memcpy(ele, &cylinder, sizeof(Fitting::Cylinder)); } } 根据上述代码实现圆柱剩余两个轴线向量
最新发布
09-03
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值