_posemath.c

CNC 的开放码,EMC2 V2.2.8版

int pmMatMatMult(PmRotationMatrix m1, PmRotationMatrix m2,
    PmRotationMatrix * mout)
{
    mout->x.x = m1.x.x * m2.x.x + m1.y.x * m2.x.y + m1.z.x * m2.x.z;
    mout->x.y = m1.x.y * m2.x.x + m1.y.y * m2.x.y + m1.z.y * m2.x.z;
    mout->x.z = m1.x.z * m2.x.x + m1.y.z * m2.x.y + m1.z.z * m2.x.z;

    mout->y.x = m1.x.x * m2.y.x + m1.y.x * m2.y.y + m1.z.x * m2.y.z;
    mout->y.y = m1.x.y * m2.y.x + m1.y.y * m2.y.y + m1.z.y * m2.y.z;
    mout->y.z = m1.x.z * m2.y.x + m1.y.z * m2.y.y + m1.z.z * m2.y.z;

    mout->z.x = m1.x.x * m2.z.x + m1.y.x * m2.z.y + m1.z.x * m2.z.z;
    mout->z.y = m1.x.y * m2.z.x + m1.y.y * m2.z.y + m1.z.y * m2.z.z;
    mout->z.z = m1.x.z * m2.z.x + m1.y.z * m2.z.y + m1.z.z * m2.z.z;

    return pmErrno = 0;
}

/* PmQuaternion functions */

int pmQuatQuatCompare(PmQuaternion q1, PmQuaternion q2)
{
#ifdef PM_DEBUG
    if (!pmQuatIsNorm(q1) || !pmQuatIsNorm(q2)) {
#ifdef PM_PRINT_ERROR
	pmPrintError("Bad quaternion in pmQuatQuatCompare\n");
#endif
    }
#endif

    if (fabs(q1.s - q2.s) < Q_FUZZ &&
	fabs(q1.x - q2.x) < Q_FUZZ &&
	fabs(q1.y - q2.y) < Q_FUZZ && fabs(q1.z - q2.z) < Q_FUZZ) {
	return 1;
    }

    /* note (0, x, y, z) = (0, -x, -y, -z) */
    if (fabs(q1.s) >= QS_FUZZ ||
	fabs(q1.x + q2.x) >= Q_FUZZ ||
	fabs(q1.y + q2.y) >= Q_FUZZ || fabs(q1.z + q2.z) >= Q_FUZZ) {
	return 0;
    }

    return 1;
}

int pmQuatMag(PmQuaternion q, double *d)
{
    PmRotationVector r;
    int r1;

    if (0 == d) {
	return (pmErrno = PM_ERR);
    }

    r1 = pmQuatRotConvert(q, &r);
    *d = r.s;

    return pmErrno = r1;
}

int pmQuatNorm(PmQuaternion q1, PmQuaternion * qout)
{
    double size = pmSqrt(pmSq(q1.s) + pmSq(q1.x) + pmSq(q1.y) + pmSq(q1.z));

    if (size == 0.0) {
#ifdef PM_PRINT_ERROR
	pmPrintError("Bad quaternion in pmQuatNorm\n");
#endif
	qout->s = 1;
	qout->x = 0;
	qout->y = 0;
	qout->z = 0;

	return pmErrno = PM_NORM_ERR;
    }

    if (q1.s >= 0.0) {
	qout->s = q1.s / size;
	qout->x = q1.x / size;
	qout->y = q1.y / size;
	qout->z = q1.z / size;

	return pmErrno = 0;
    } else {
	qout->s = -q1.s / size;
	qout->x = -q1.x / size;
	qout->y = -q1.y / size;
	qout->z = -q1.z / size;

	return pmErrno = 0;
    }
}

int pmQuatInv(PmQuaternion q1, PmQuaternion * qout)
{
    if (qout == 0) {
	return pmErrno = PM_ERR;
    }

    qout->s = q1.s;
    qout->x = -q1.x;
    qout->y = -q1.y;
    qout->z = -q1.z;

#ifdef PM_DEBUG
    if (!pmQuatIsNorm(q1)) {
#ifdef PM_PRINT_ERROR
	pmPrintError("Bad quaternion in pmQuatInv\n");
#endif
	return pmErrno = PM_NORM_ERR;
    }
#endif

    return pmErrno = 0;
}

int pmQuatIsNorm(PmQuaternion q1)
{
    return (fabs(pmSq(q1.s) + pmSq(q1.x) + pmSq(q1.y) + pmSq(q1.z) - 1.0) <
	UNIT_QUAT_FUZZ);
}

int pmQuatScalMult(PmQuaternion q, double s, PmQuaternion * qout)
{
    /*! \todo FIXME-- need a native version; this goes through a rotation vector */
    PmRotationVector r;
    int r1, r2, r3;

    r1 = pmQuatRotConvert(q, &r);
    r2 = pmRotScalMult(r, s, &r);
    r3 = pmRotQuatConvert(r, qout);

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

int pmQuatScalDiv(PmQuaternion q, double s, PmQuaternion * qout)
{
    /*! \todo FIXME-- need a native version; this goes through a rotation vector */
    PmRotationVector r;
    int r1, r2, r3;

    r1 = pmQuatRotConvert(q, &r);
    r2 = pmRotScalDiv(r, s, &r);
    r3 = pmRotQuatConvert(r, qout);

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

int pmQuatQuatMult(PmQuaternion q1, PmQuaternion q2, PmQuaternion * qout)
{
    if (qout == 0) {
	return pmErrno = PM_ERR;
    }

    qout->s = q1.s * q2.s - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;

    if (qout->s >= 0.0) {
	qout->x = q1.s * q2.x + q1.x * q2.s + q1.y * q2.z - q1.z * q2.y;
	qout->y = q1.s * q2.y - q1.x * q2.z + q1.y * q2.s + q1.z * q2.x;
	qout->z = q1.s * q2.z + q1.x * q2.y - q1.y * q2.x + q1.z * q2.s;
    } else {
	qout->s *= -1;
	qout->x = -q1.s * q2.x - q1.x * q2.s - q1.y * q2.z + q1.z * q2.y;
	qout->y = -q1.s * q2.y + q1.x * q2.z - q1.y * q2.s - q1.z * q2.x;
	qout->z = -q1.s * q2.z - q1.x * q2.y + q1.y * q2.x - q1.z * q2.s;
    }

#ifdef PM_DEBUG
    if (!pmQuatIsNorm(q1) || !pmQuatIsNorm(q2)) {
#ifdef PM_PRINT_ERROR
	pmPrintError("Bad quaternion in pmQuatQuatMult\n");
#endif
	return pmErrno = PM_NORM_ERR;
    }
#endif

    return pmErrno = 0;
}

int pmQuatCartMult(PmQuaternion q1, PmCartesian v2, PmCartesian * vout)
{
    PmCartesian c;

    c.x = q1.y * v2.z - q1.z * v2.y;
    c.y = q1.z * v2.x - q1.x * v2.z;
    c.z = q1.x * v2.y - q1.y * v2.x;

    vout->x = v2.x + 2.0 * (q1.s * c.x + q1.y * c.z - q1.z * c.y);
    vout->y = v2.y + 2.0 * (q1.s * c.y + q1.z * c.x - q1.x * c.z);
    vout->z = v2.z + 2.0 * (q1.s * c.z + q1.x * c.y - q1.y * c.x);

#ifdef PM_DEBUG
    if (!pmQuatIsNorm(q1)) {
#ifdef PM_PRINT_ERROR
	pmPrintError("Bad quaternion in pmQuatCartMult\n");
#endif
	return pmErrno = PM_NORM_ERR;
    }
#endif

    return pmErrno = 0;
}

/* PmPose functions*/

int pmPosePoseCompare(PmPose p1, PmPose p2)
{
#ifdef PM_DEBUG
    if (!pmQuatIsNorm(p1.rot) || !pmQuatIsNorm(p2.rot)) {
#ifdef PM_PRINT_ERROR
	pmPrintError("Bad quaternion in pmPosePoseCompare\n");
#endif
    }
#endif

    return pmErrno = (pmQuatQuatCompare(p1.rot, p2.rot) &&
	pmCartCartCompare(p1.tran, p2.tran));
}

int pmPoseInv(PmPose p1, PmPose * p2)
{
    int r1, r2;

#ifdef PM_DEBUG
    if (!pmQuatIsNorm(p1.rot)) {
#ifdef PM_PRINT_ERROR
	pmPrintError("Bad quaternion in pmPoseInv\n");
#endif
    }
#endif

    r1 = pmQuatInv(p1.rot, &p2->rot);
    r2 = pmQuatCartMult(p2->rot, p1.tran, &p2->tran);

    p2->tran.x *= -1.0;
    p2->tran.y *= -1.0;
    p2->tran.z *= -1.0;

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

int pmPoseCartMult(PmPose p1, PmCartesian v2, PmCartesian * vout)
{
    int r1, r2;

#ifdef PM_DEBUG
    if (!pmQuatIsNorm(p1.rot)) {
#ifdef PM_PRINT_ERROR
	pmPrintError("Bad quaternion in pmPoseCartMult\n");
#endif
	return pmErrno = PM_NORM_ERR;
    }
#endif

    r1 = pmQuatCartMult(p1.rot, v2, vout);
    r2 = pmCartCartAdd(p1.tran, *vout, vout);

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

int pmPosePoseMult(PmPose p1, PmPose p2, PmPose * pout)
{
    int r1, r2, r3;

#ifdef PM_DEBUG
    if (!pmQuatIsNorm(p1.rot) || !pmQuatIsNorm(p2.rot)) {
#ifdef PM_PRINT_ERROR
	pmPrintError("Bad quaternion in pmPosePoseMult\n");
#endif
	return pmErrno = PM_NORM_ERR;
    }
#endif

    r1 = pmQuatCartMult(p1.rot, p2.tran, &pout->tran);
    r2 = pmCartCartAdd(p1.tran, pout->tran, &pout->tran);
    r3 = pmQuatQuatMult(p1.rot, p2.rot, &pout->rot);

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

/* homogeneous transform functions */

int pmHomInv(PmHomogeneous h1, PmHomogeneous * h2)
{
    int r1, r2;

#ifdef PM_DEBUG
    if (!pmMatIsNorm(h1.rot)) {
#ifdef PM_PRINT_ERROR
	pmPrintError("Bad rotation matrix in pmHomInv\n");
#endif
    }
#endif

    r1 = pmMatInv(h1.rot, &h2->rot);
    r2 = pmMatCartMult(h2->rot, h1.tran, &h2->tran);

    h2->tran.x *= -1.0;
    h2->tran.y *= -1.0;
    h2->tran.z *= -1.0;

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

/* line functions */

int pmLineInit(PmLine * line, PmPose start, PmPose end)
{
    int r1 = 0, r2 = 0, r3 = 0, r4 = 0, r5 = 0;
    double tmag = 0.0;
    double rmag = 0.0;
    PmQuaternion startQuatInverse;

    if (0 == line) {
	return (pmErrno = PM_ERR);
    }

    r3 = pmQuatInv(start.rot, &startQuatInverse);
    if (r3) {
	return r3;
    }

    r4 = pmQuatQuatMult(startQuatInverse, end.rot, &line->qVec);
    if (r4) {
	return r4;
    }

    pmQuatMag(line->qVec, &rmag);
    if (rmag > Q_FUZZ) {
	r5 = pmQuatScalMult(line->qVec, 1 / rmag, &(line->qVec));
	if (r5) {
	    return r5;
	}
    }

    line->start = start;
    line->end = end;
    r1 = pmCartCartSub(end.tran, start.tran, &line->uVec);
    if (r1) {
	return r1;
    }

    pmCartMag(line->uVec, &tmag);
    if (IS_FUZZ(tmag, CART_FUZZ)) {
	line->uVec.x = 1.0;
	line->uVec.y = 0.0;
	line->uVec.z = 0.0;
    } else {
	r2 = pmCartUnit(line->uVec, &line->uVec);
    }
    line->tmag = tmag;
    line->rmag = rmag;
    line->tmag_zero = (line->tmag <= CART_FUZZ);
    line->rmag_zero = (line->rmag <= Q_FUZZ);

    /* return PM_NORM_ERR if uVec has been set to 1, 0, 0 */
    return pmErrno = (r1 || r2 || r3 || r4 || r5) ? PM_NORM_ERR : 0;
}

int pmLinePoint(PmLine * line, double len, PmPose * point)
{
    int r1 = 0, r2 = 0, r3 = 0, r4 = 0;

    if (line->tmag_zero) {
	point->tran = line->end.tran;
    } else {
	/* return start + len * uVec */
	r1 = pmCartScalMult(line->uVec, len, &point->tran);
	r2 = pmCartCartAdd(line->start.tran, point->tran, &point->tran);
    }

    if (line->rmag_zero) {
	point->rot = line->end.rot;
    } else {
	if (line->tmag_zero) {
	    r3 = pmQuatScalMult(line->qVec, len, &point->rot);
	} else {
	    r3 = pmQuatScalMult(line->qVec, len * line->rmag / line->tmag,
		&point->rot);
	}
	r4 = pmQuatQuatMult(line->start.rot, point->rot, &point->rot);
    }

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

/* circle functions */

/*
  pmCircleInit() takes the defining parameters of a generalized circle
  and sticks them in the structure. It also computes the radius and vectors
  in the plane that are useful for other functions and that don't need
  to be recomputed every time.

  Note that the end can be placed arbitrarily, resulting in a combination of
  spiral and helical motion. There is an overconstraint between the start,
  center, and normal vector: the center vector and start vector are assumed
  to be in the plane defined by the normal vector. If this is not true, then
  it will be made true by moving the center vector onto the plane.
  */
int pmCircleInit(PmCircle * circle,
    PmPose start, PmPose end,
    PmCartesian center, PmCartesian normal, int turn)
{
    double dot;
    PmCartesian rEnd;
    PmCartesian v;
    double d;
    int r1;

#ifdef PM_DEBUG
    if (0 == circle) {
#ifdef PM_PRINT_ERROR
	pmPrintError("error: pmCircleInit cirle pointer is null\n");
#endif
	return pmErrno = PM_ERR;
    }
#endif

    /* adjust center */
    pmCartCartSub(start.tran, center, &v);
    r1 = pmCartCartProj(v, normal, &v);
    if (PM_NORM_ERR == r1) {
	/* bad normal vector-- abort */
#ifdef PM_PRINT_ERROR
	pmPrintError("error: pmCircleInit normal vector is 0\n");
#endif
	return -1;
    }
    pmCartCartAdd(v, center, &circle->center);

    /* normalize and redirect normal vector based on turns. If turn is less
       than 0, point normal vector in other direction and make turn positive, 
       -1 -> 0, -2 -> 1, etc. */
    pmCartUnit(normal, &circle->normal);
    if (turn < 0) {
	turn = -1 - turn;
	pmCartScalMult(circle->normal, -1.0, &circle->normal);
    }

    /* radius */
    pmCartCartDisp(start.tran, circle->center, &circle->radius);

    /* vector in plane of circle from center to start, magnitude radius */
    pmCartCartSub(start.tran, circle->center, &circle->rTan);
    /* vector in plane of circle perpendicular to rTan, magnitude radius */
    pmCartCartCross(circle->normal, circle->rTan, &circle->rPerp);

    /* do rHelix, rEnd */
    pmCartCartSub(end.tran, circle->center, &circle->rHelix);
    pmCartPlaneProj(circle->rHelix, circle->normal, &rEnd);
    pmCartMag(rEnd, &circle->spiral);
    circle->spiral -= circle->radius;
    pmCartCartSub(circle->rHelix, rEnd, &circle->rHelix);
    pmCartUnit(rEnd, &rEnd);
    pmCartScalMult(rEnd, circle->radius, &rEnd);

    /* Patch for error spiral end same as spiral center */
    pmCartMag(rEnd, &d);
    if (d == 0.0) {
	pmCartScalMult(circle->normal, DOUBLE_FUZZ, &v);
	pmCartCartAdd(rEnd, v, &rEnd);
    }
    /* end patch 03-mar-1999 Dirk Maij */

    /* angle */
    pmCartCartDot(circle->rTan, rEnd, &dot);
    dot = dot / (circle->radius * circle->radius);
    if (dot > 1.0) {
	circle->angle = 0.0;
    } else if (dot < -1.0) {
	circle->angle = PM_PI;
    } else {
	circle->angle = acos(dot);
    }
    /* now angle is in range 0..PI . Check if cross is antiparallel to
       normal. If so, true angle is between PI..2PI. Need to subtract from
       2PI. */
    pmCartCartCross(circle->rTan, rEnd, &v);
    pmCartCartDot(v, circle->normal, &d);
    if (d < 0.0) {
	circle->angle = PM_2_PI - circle->angle;
    }

    if (circle->angle > -(CIRCLE_FUZZ) && circle->angle < (CIRCLE_FUZZ)) {
	circle->angle = PM_2_PI;
    }

    /* now add more angle for multi turns */
    if (turn > 0) {
	circle->angle += turn * 2.0 * PM_PI;
    }
/*! \todo Another #if 0*/
#if 0
    printf("\n\n");
    printf("pmCircleInit:\n");
    printf(" \t start  : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
	start.tran.x, start.tran.y, start.tran.z);
    printf(" \t end    : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
	end.tran.x, end.tran.y, end.tran.z);
    printf(" \t center : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
	center.x, center.y, center.z);
    printf(" \t normal : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
	normal.x, normal.y, normal.z);
    printf(" \t rEnd   : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
	rEnd.x, rEnd.y, rEnd.z);
    printf(" \t turn=%d\n", turn);
    printf(" \t dot=%9.9f\n", dot);
    printf(" \t d=%9.9f\n", d);
    printf(" \t circle  \t{angle=%9.9f, radius=%9.9f, spiral=%9.9f}\n",
	circle->angle, circle->radius, circle->spiral);
    printf(" \t circle->normal : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
	circle->normal.x, circle->normal.y, circle->normal.z);
    printf(" \t circle->center : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
	circle->center.x, circle->center.y, circle->center.z);
    printf(" \t circle->rTan : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
	circle->rTan.x, circle->rTan.y, circle->rTan.z);
    printf(" \t circle->rPerp : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
	circle->rPerp.x, circle->rPerp.y, circle->rPerp.z);
    printf(" \t circle->rHelix : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
	circle->rHelix.x, circle->rHelix.y, circle->rHelix.z);
    printf("\n\n");
#endif

    return pmErrno = 0;
}

/*
  pmCirclePoint() returns the point at the given angle along
  the circle. If the circle is a helix or spiral or combination, the
  point will include interpolation off the actual circle.
  */
int pmCirclePoint(PmCircle * circle, double angle, PmPose * point)
{
    PmCartesian par, perp;
    double scale;

#ifdef PM_DEBUG
    if (0 == circle || 0 == point) {
#ifdef PM_PRINT_ERROR
	pmPrintError
	    ("error: pmCirclePoint circle or point pointer is null\n");
#endif
	return pmErrno = PM_ERR;
    }
#endif

    /* compute components rel to center */
    pmCartScalMult(circle->rTan, cos(angle), &par);
    pmCartScalMult(circle->rPerp, sin(angle), &perp);

    /* add to get radius vector rel to center */
    pmCartCartAdd(par, perp, &point->tran);

    /* get scale for spiral, helix interpolation */
    if (circle->angle == 0.0) {
#ifdef PM_PRINT_ERROR
	pmPrintError("error: pmCirclePoint angle is zero\n");
#endif
	return pmErrno = PM_DIV_ERR;
    }
    scale = angle / circle->angle;

    /* add scaled vector in radial dir for spiral */
    pmCartUnit(point->tran, &par);
    pmCartScalMult(par, scale * circle->spiral, &par);
    pmCartCartAdd(point->tran, par, &point->tran);

    /* add scaled vector in helix dir */
    pmCartScalMult(circle->rHelix, scale, &perp);
    pmCartCartAdd(point->tran, perp, &point->tran);

    /* add to center vector for final result */
    pmCartCartAdd(circle->center, point->tran, &point->tran);

    return pmErrno = 0;
}