_posemath.c

CNC 的开放码,EMC2 V2.2.8版

    return pmErrno = r1 || r2 ? PM_NORM_ERR : 0;
}

int pmZyzMatConvert(PmEulerZyz zyz, PmRotationMatrix * m)
{
    double sa, sb, sg;
    double ca, cb, cg;

    sa = sin(zyz.z);
    sb = sin(zyz.y);
    sg = sin(zyz.zp);

    ca = cos(zyz.z);
    cb = cos(zyz.y);
    cg = cos(zyz.zp);

    m->x.x = ca * cb * cg - sa * sg;
    m->y.x = -ca * cb * sg - sa * cg;
    m->z.x = ca * sb;

    m->x.y = sa * cb * cg + ca * sg;
    m->y.y = -sa * cb * sg + ca * cg;
    m->z.y = sa * sb;

    m->x.z = -sb * cg;
    m->y.z = sb * sg;
    m->z.z = cb;

    return pmErrno = 0;
}

int pmZyzRpyConvert(PmEulerZyz zyz, PmRpy * rpy)
{
#ifdef PM_PRINT_ERROR
    pmPrintError("error: pmZyzRpyConvert not implemented\n");
#endif
    return pmErrno = PM_IMPL_ERR;
}

int pmZyxRotConvert(PmEulerZyx zyx, PmRotationVector * r)
{
    PmRotationMatrix m;
    int r1, r2;

    /*! \todo FIXME-- need direct equations */
    r1 = pmZyxMatConvert(zyx, &m);
    r2 = pmMatRotConvert(m, r);

    return pmErrno = r1 || r2 ? PM_NORM_ERR : 0;
}

int pmZyxQuatConvert(PmEulerZyx zyx, PmQuaternion * q)
{
    PmRotationMatrix m;
    int r1, r2;

    /*! \todo FIXME-- need direct equations */
    r1 = pmZyxMatConvert(zyx, &m);
    r2 = pmMatQuatConvert(m, q);

    return pmErrno = r1 || r2 ? PM_NORM_ERR : 0;
}

int pmZyxMatConvert(PmEulerZyx zyx, PmRotationMatrix * m)
{
    double sa, sb, sg;
    double ca, cb, cg;

    sa = sin(zyx.z);
    sb = sin(zyx.y);
    sg = sin(zyx.x);

    ca = cos(zyx.z);
    cb = cos(zyx.y);
    cg = cos(zyx.x);

    m->x.x = ca * cb;
    m->y.x = ca * sb * sg - sa * cg;
    m->z.x = ca * sb * cg + sa * sg;

    m->x.y = sa * cb;
    m->y.y = sa * sb * sg + ca * cg;
    m->z.y = sa * sb * cg - ca * sg;

    m->x.z = -sb;
    m->y.z = cb * sg;
    m->z.z = cb * cg;

    return pmErrno = 0;
}

int pmZyxZyzConvert(PmEulerZyx zyx, PmEulerZyz * zyz)
{
#ifdef PM_PRINT_ERROR
    pmPrintError("error: pmZyxZyzConvert not implemented\n");
#endif
    return pmErrno = PM_IMPL_ERR;
}

int pmZyxRpyConvert(PmEulerZyx zyx, PmRpy * rpy)
{
#ifdef PM_PRINT_ERROR
    pmPrintError("error: pmZyxRpyConvert not implemented\n");
#endif
    return pmErrno = PM_IMPL_ERR;
}

int pmRpyRotConvert(PmRpy rpy, PmRotationVector * r)
{
    PmQuaternion q;
    int r1, r2;
    r1 = 0;
    r2 = 0;
    q.s = q.x = q.y = q.z = 0.0;
    r->s = r->x = r->y = r->z = 0.0;

    r1 = pmRpyQuatConvert(rpy, &q);
    r2 = pmQuatRotConvert(q, r);

    return r1 || r2 ? pmErrno : 0;
}

int pmRpyQuatConvert(PmRpy rpy, PmQuaternion * q)
{
    PmRotationMatrix m;
    int r1, r2;

    /*! \todo FIXME-- need direct equations */
    r1 = pmRpyMatConvert(rpy, &m);
    r2 = pmMatQuatConvert(m, q);

    return pmErrno = r1 || r2 ? PM_NORM_ERR : 0;
}

int pmRpyMatConvert(PmRpy rpy, PmRotationMatrix * m)
{
    double sa, sb, sg;
    double ca, cb, cg;

    sa = sin(rpy.y);
    sb = sin(rpy.p);
    sg = sin(rpy.r);

    ca = cos(rpy.y);
    cb = cos(rpy.p);
    cg = cos(rpy.r);

    m->x.x = ca * cb;
    m->y.x = ca * sb * sg - sa * cg;
    m->z.x = ca * sb * cg + sa * sg;

    m->x.y = sa * cb;
    m->y.y = sa * sb * sg + ca * cg;
    m->z.y = sa * sb * cg - ca * sg;

    m->x.z = -sb;
    m->y.z = cb * sg;
    m->z.z = cb * cg;

    return pmErrno = 0;
}

int pmRpyZyzConvert(PmRpy rpy, PmEulerZyz * zyz)
{
#ifdef PM_PRINT_ERROR
    pmPrintError("error: pmRpyZyzConvert not implemented\n");
#endif
    return pmErrno = PM_IMPL_ERR;
}

int pmRpyZyxConvert(PmRpy rpy, PmEulerZyx * zyx)
{
#ifdef PM_PRINT_ERROR
    pmPrintError("error: pmRpyZyxConvert not implemented\n");
#endif
    return pmErrno = PM_IMPL_ERR;
}

int pmPoseHomConvert(PmPose p, PmHomogeneous * h)
{
    int r1;

    h->tran = p.tran;
    r1 = pmQuatMatConvert(p.rot, &h->rot);

    return pmErrno = r1;
}

int pmHomPoseConvert(PmHomogeneous h, PmPose * p)
{
    int r1;

    p->tran = h.tran;
    r1 = pmMatQuatConvert(h.rot, &p->rot);

    return pmErrno = r1;
}

/* PmCartesian functions */

int pmCartCartCompare(PmCartesian v1, PmCartesian v2)
{
    if (fabs(v1.x - v2.x) >= V_FUZZ ||
	fabs(v1.y - v2.y) >= V_FUZZ || fabs(v1.z - v2.z) >= V_FUZZ) {
	return 0;
    }

    return 1;
}

int pmCartCartDot(PmCartesian v1, PmCartesian v2, double *d)
{
    *d = v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;

    return pmErrno = 0;
}

int pmCartCartCross(PmCartesian v1, PmCartesian v2, PmCartesian * vout)
{
    vout->x = v1.y * v2.z - v1.z * v2.y;
    vout->y = v1.z * v2.x - v1.x * v2.z;
    vout->z = v1.x * v2.y - v1.y * v2.x;

    return pmErrno = 0;
}

int pmCartMag(PmCartesian v, double *d)
{
    *d = pmSqrt(pmSq(v.x) + pmSq(v.y) + pmSq(v.z));

    return pmErrno = 0;
}

int pmCartCartDisp(PmCartesian v1, PmCartesian v2, double *d)
{
    *d = pmSqrt(pmSq(v2.x - v1.x) + pmSq(v2.y - v1.y) + pmSq(v2.z - v1.z));

    return pmErrno = 0;
}

int pmCartCartAdd(PmCartesian v1, PmCartesian v2, PmCartesian * vout)
{
    vout->x = v1.x + v2.x;
    vout->y = v1.y + v2.y;
    vout->z = v1.z + v2.z;

    return pmErrno = 0;
}

int pmCartCartSub(PmCartesian v1, PmCartesian v2, PmCartesian * vout)
{
    vout->x = v1.x - v2.x;
    vout->y = v1.y - v2.y;
    vout->z = v1.z - v2.z;

    return pmErrno = 0;
}

int pmCartScalMult(PmCartesian v1, double d, PmCartesian * vout)
{
    vout->x = v1.x * d;
    vout->y = v1.y * d;
    vout->z = v1.z * d;

    return pmErrno = 0;
}

int pmCartScalDiv(PmCartesian v1, double d, PmCartesian * vout)
{
    if (d == 0.0) {
#ifdef PM_PRINT_ERROR
	pmPrintError("Divide by 0 in pmCartScalDiv\n");
#endif
	vout->x = DBL_MAX;
	vout->y = DBL_MAX;
	vout->z = DBL_MAX;

	return pmErrno = PM_DIV_ERR;
    }

    vout->x = v1.x / d;
    vout->y = v1.y / d;
    vout->z = v1.z / d;

    return pmErrno = 0;
}

int pmCartNeg(PmCartesian v1, PmCartesian * vout)
{
    vout->x = -v1.x;
    vout->y = -v1.y;
    vout->z = -v1.z;

    return pmErrno = 0;
}

int pmCartInv(PmCartesian v1, PmCartesian * vout)
{
    double size_sq = pmSq(v1.x) + pmSq(v1.y) + pmSq(v1.z);

    if (size_sq == 0.0) {
#ifdef PM_PRINT_ERROR
	pmPrintError("Zero vector in pmCartInv\n");
#endif

	vout->x = DBL_MAX;
	vout->y = DBL_MAX;
	vout->z = DBL_MAX;

	return pmErrno = PM_NORM_ERR;
    }

    vout->x = v1.x / size_sq;
    vout->y = v1.y / size_sq;
    vout->z = v1.z / size_sq;

    return pmErrno = 0;
}

// This used to be called pmCartNorm.

int pmCartUnit(PmCartesian v, PmCartesian * vout)
{
    double size = pmSqrt(pmSq(v.x) + pmSq(v.y) + pmSq(v.z));

    if (size == 0.0) {
#ifdef PM_PRINT_ERROR
	pmPrintError("Zero vector in pmCartUnit\n");
#endif

	vout->x = DBL_MAX;
	vout->y = DBL_MAX;
	vout->z = DBL_MAX;

	return pmErrno = PM_NORM_ERR;
    }

    vout->x = v.x / size;
    vout->y = v.y / size;
    vout->z = v.z / size;

    return pmErrno = 0;
}

/*! \todo This is if 0'd out so we can find all the pmCartNorm calls that should
 be renamed pmCartUnit. 
 Later we'll put this back. */
#if 0

int pmCartNorm(PmCartesian v, PmCartesian * vout)
{

    vout->x = v.x;
    vout->y = v.y;
    vout->z = v.z;

    return pmErrno = 0;
}
#endif

int pmCartIsNorm(PmCartesian v)
{
    return pmSqrt(pmSq(v.x) + pmSq(v.y) + pmSq(v.z)) - 1.0 <
	UNIT_VEC_FUZZ ? 1 : 0;
}

int pmCartCartProj(PmCartesian v1, PmCartesian v2, PmCartesian * vout)
{
    int r1, r2, r3;
    double d;

    r1 = pmCartUnit(v2, &v2);
    r2 = pmCartCartDot(v1, v2, &d);
    r3 = pmCartScalMult(v2, d, vout);

    return pmErrno = r1 || r2 || r3 ? PM_NORM_ERR : 0;
}

int pmCartPlaneProj(PmCartesian v, PmCartesian normal, PmCartesian * vout)
{
    int r1, r2;
    PmCartesian par;

    r1 = pmCartCartProj(v, normal, &par);
    r2 = pmCartCartSub(v, par, vout);

    return pmErrno = r1 || r2 ? PM_NORM_ERR : 0;
}

/* angle-axis functions */

int pmQuatAxisAngleMult(PmQuaternion q, PmAxis axis, double angle,
    PmQuaternion * pq)
{
    double sh, ch;

#ifdef PM_DEBUG
    if (!pmQuatIsNorm(q)) {
#ifdef PM_PRINT_ERROR
	pmPrintError("error: non-unit quaternion in pmQuatAxisAngleMult\n");
#endif
	return -1;
    }
#endif

    angle *= 0.5;
    sincos(angle, &sh, &ch);

    switch (axis) {
    case PM_X:
	pq->s = ch * q.s - sh * q.x;
	pq->x = ch * q.x + sh * q.s;
	pq->y = ch * q.y + sh * q.z;
	pq->z = ch * q.z - sh * q.y;
	break;

    case PM_Y:
	pq->s = ch * q.s - sh * q.y;
	pq->x = ch * q.x - sh * q.z;
	pq->y = ch * q.y + sh * q.s;
	pq->z = ch * q.z + sh * q.x;
	break;

    case PM_Z:
	pq->s = ch * q.s - sh * q.z;
	pq->x = ch * q.x + sh * q.y;
	pq->y = ch * q.y - sh * q.x;
	pq->z = ch * q.z + sh * q.s;
	break;

    default:
#ifdef PM_PRINT_ERROR
	pmPrintError("error: bad axis in pmQuatAxisAngleMult\n");
#endif
	return -1;
	break;
    }

    if (pq->s < 0.0) {
	pq->s *= -1.0;
	pq->x *= -1.0;
	pq->y *= -1.0;
	pq->z *= -1.0;
    }

    return 0;
}

/* PmRotationVector functions */

int pmRotScalMult(PmRotationVector r, double s, PmRotationVector * rout)
{
    rout->s = r.s * s;
    rout->x = r.x;
    rout->y = r.y;
    rout->z = r.z;

    return pmErrno = 0;
}

int pmRotScalDiv(PmRotationVector r, double s, PmRotationVector * rout)
{
    if (s == 0.0) {
#ifdef PM_PRINT_ERROR
	pmPrintError("Divide by zero in pmRotScalDiv\n");
#endif

	rout->s = DBL_MAX;
	rout->x = r.x;
	rout->y = r.y;
	rout->z = r.z;

	return pmErrno = PM_NORM_ERR;
    }

    rout->s = r.s / s;
    rout->x = r.x;
    rout->y = r.y;
    rout->z = r.z;

    return pmErrno = 0;
}

int pmRotIsNorm(PmRotationVector r)
{
    if (fabs(r.s) < RS_FUZZ ||
	fabs(pmSqrt(pmSq(r.x) + pmSq(r.y) + pmSq(r.z))) - 1.0 < UNIT_VEC_FUZZ)
    {
	return 1;
    }

    return 0;
}

int pmRotNorm(PmRotationVector r, PmRotationVector * rout)
{
    double size;

    size = pmSqrt(pmSq(r.x) + pmSq(r.y) + pmSq(r.z));

    if (fabs(r.s) < RS_FUZZ) {
	rout->s = 0.0;
	rout->x = 0.0;
	rout->y = 0.0;
	rout->z = 0.0;

	return pmErrno = 0;
    }

    if (size == 0.0) {
#ifdef PM_PRINT_ERROR
	pmPrintError("error: pmRotNorm size is zero\n");
#endif

	rout->s = 0.0;
	rout->x = 0.0;
	rout->y = 0.0;
	rout->z = 0.0;

	return pmErrno = PM_NORM_ERR;
    }

    rout->s = r.s;
    rout->x = r.x / size;
    rout->y = r.y / size;
    rout->z = r.z / size;

    return pmErrno = 0;
}

/* PmRotationMatrix functions */

int pmMatNorm(PmRotationMatrix m, PmRotationMatrix * mout)
{
    /*! \todo FIXME */
    *mout = m;

#ifdef PM_PRINT_ERROR
    pmPrintError("error: pmMatNorm not implemented\n");
#endif
    return pmErrno = PM_IMPL_ERR;
}

int pmMatIsNorm(PmRotationMatrix m)
{
    PmCartesian u;

    pmCartCartCross(m.x, m.y, &u);

    return (pmCartIsNorm(m.x) &&
	pmCartIsNorm(m.y) && pmCartIsNorm(m.z) && pmCartCartCompare(u, m.z));
}

int pmMatInv(PmRotationMatrix m, PmRotationMatrix * mout)
{
    /* inverse of a rotation matrix is the transpose */

    mout->x.x = m.x.x;
    mout->x.y = m.y.x;
    mout->x.z = m.z.x;

    mout->y.x = m.x.y;
    mout->y.y = m.y.y;
    mout->y.z = m.z.y;

    mout->z.x = m.x.z;
    mout->z.y = m.y.z;
    mout->z.z = m.z.z;

    return pmErrno = 0;
}

int pmMatCartMult(PmRotationMatrix m, PmCartesian v, PmCartesian * vout)
{
    vout->x = m.x.x * v.x + m.y.x * v.y + m.z.x * v.z;
    vout->y = m.x.y * v.x + m.y.y * v.y + m.z.y * v.z;
    vout->z = m.x.z * v.x + m.y.z * v.y + m.z.z * v.z;

    return pmErrno = 0;
}