c语言 中点法则求pi,中点求导公式计算轨迹法向量

template struct Point3_ {

Point3_() : x(0), y(0), z(0) {}

Point3_(T _x, T _y, T _z) : x(_x), y(_y), z(_z) {}

T x;

T y;

T z;

};

typedef Point3_ Point3I;

typedef Point3_ Point3F;

typedef Point3_ Point3D;

double CalNormalAngle(

IN const Point3D &pt0, IN const Point3D &pt1, IN const Point3D &pt2) {

Q_UNUSED(pt1)

double angle = 0.0;

double dx = pt2.x - pt0.x;

double dy = pt2.y - pt0.y;

if (fabs(dx) <= _EPS_) {

angle = 0;

} else {

if (fabs(dy) <= _EPS_) {

angle = 90;

} else {

angle = (-atan(dx / dy)) / _PI_ * 180;

}

}

return angle;

}

const static int length = 5.0;

void Esvi3DGraph::CalPointsNormal(IN const std::vector &points,

OUT std::vector &normalVec,

OUT std::vector<:pair point3d>> &normalLines) {

int ptSize = points.size();

if (points.size() > 1) {

//计算质心

double sumX = 0.0;

double sumY = 0.0;

double sumZ = 0.0;

for (int i = 0; i < ptSize; ++i) {

const Point3D &p3d = points[i];

sumX += p3d.x;

sumY += p3d.y;

sumZ += p3d.z;

}

double avgX = sumX / ptSize;

double avgY = sumY / ptSize;

double avgZ = sumZ / ptSize;

//计算质心

std::vector clonePoints = points; //复制的点

const Point3D &firstPoint = points[0];

const Point3D &endPoint = points[ptSize - 1];

clonePoints.insert(clonePoints.begin(), endPoint);

clonePoints.insert(clonePoints.end(), firstPoint);

for (int i = 0; i < clonePoints.size() - 2; ++i) {

const Point3D &p3d0 = clonePoints[i];

const Point3D &p3d1 = clonePoints[i + 1];

const Point3D &p3d2 = clonePoints[i + 2];

//计算p3d1的法向量角度

double angle = CalNormalAngle(p3d0, p3d1, p3d2);

Point3D vPoint;

Point3D v0, v1, v3;

v3.x = avgX - p3d1.x;

v3.y = avgY - p3d1.y;

v3.z = avgZ - p3d1.z;

std::pair line;

line.first = p3d1;

if (angle == 90.0) {

v0.x = 0;

v0.y = m_pointTestEps;

v0.z = p3d1.z;

v1.x = 0;

v1.y = -m_pointTestEps;

v1.z = p3d1.z;

if (Product(v0, v3) >= 0) {

vPoint = v0;

} else {

vPoint = v1;

}

} else {

Point3D p0, p1;

double k = tan(angle / 180.0 * _PI_);

//求指向内部的法向量，y=kx+b

double b = p3d1.y - k * p3d1.x;

//构造向量

if (fabs(k) <= 1.0) {

v0.x = p3d1.x + m_pointTestEps;

v0.y = k * v0.x + b;

v0.z = p3d1.z;

p0 = v0;

v0.x -= p3d1.x;

v0.y -= p3d1.y;

v1.x = p3d1.x - m_pointTestEps;

v1.y = k * v1.x + b;

v1.z = p3d1.z;

p1 = v1;

v1.x -= p3d1.x;

v1.y -= p3d1.y;

} else {

v0.y = p3d1.y + m_pointTestEps;

v0.x = (v0.y - b) / k;

v0.z = p3d1.z;

p0 = v0;

v0.x -= p3d1.x;

v0.y -= p3d1.y;

v1.y = p3d1.y - m_pointTestEps;

v1.x = (v1.y - b) / k;

v1.z = p3d1.z;

p1 = v1;

v1.x -= p3d1.x;

v1.y -= p3d1.y;

}

bool result0 = PointInPolygon(points, p0);

if (result0) {

vPoint = v0;

} else {

vPoint = v1;

}

}

//归一化向量

double vLen = sqrt(vPoint.x * vPoint.x + vPoint.y * vPoint.y);

vPoint.x /= vLen;

vPoint.y /= vLen;

Point3D secondPt;

//单位向量乘长度，来显示向量

secondPt.x = line.first.x + vPoint.x * length ;

secondPt.y = line.first.y + vPoint.y * length ;

secondPt.z = line.first.z;

line.second = secondPt;

//存储向量

normalLines.push_back(line);

normalVec.push_back(vPoint);

}

//如果起始点和终止点位置相同，法向量相等

if (fabs(firstPoint.x - endPoint.x) <= _EPS_ &&

fabs(firstPoint.y - endPoint.y) <= _EPS_) {

normalVec[0] = normalVec[normalVec.size() - 1];

normalLines[0] = normalLines[normalLines.size() - 1];

}

}

}