posemath.cc

CNC 的开放码,EMC2 V2.2.8版

    return p;
}

// PM_HOMOGENEOUS class

PM_HOMOGENEOUS::PM_HOMOGENEOUS(PM_CARTESIAN v, PM_ROTATION_MATRIX m)
{
    tran = v;
    rot = m;
}

PM_HOMOGENEOUS::PM_HOMOGENEOUS(PM_CONST PM_POSE PM_REF p)
{
    PmPose pose;
    PmHomogeneous hom;

    toPose(p, &pose);
    pmPoseHomConvert(pose, &hom);
    toHom(hom, this);
}

PM_CARTESIAN & PM_HOMOGENEOUS::operator [](int n) {
    // if it is a rotation vector, stuff 0 as default bottom
    // if it is a translation vector, stuff 1 as default bottom

    switch (n) {
    case 0:
	noElement = 0.0;
	return rot.x;
    case 1:
	noElement = 0.0;
	return rot.y;
    case 2:
	noElement = 0.0;
	return rot.z;
    case 3:
	noElement = 1.0;
	return tran;
    default:
	if (0 == noCart) {
	    noCart = new PM_CARTESIAN(0.0, 0.0, 0.0);
	}
	return (*noCart);	// need to return a PM_CARTESIAN &
    }
}

#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS
PM_HOMOGENEOUS::PM_HOMOGENEOUS(PM_CCONST PM_HOMOGENEOUS & h)
{
    tran = h.tran;
    rot = h.rot;
}
#endif

PM_HOMOGENEOUS PM_HOMOGENEOUS::operator =(PM_HOMOGENEOUS h)
{
    tran = h.tran;
    rot = h.rot;

    return h;
}

// PM_LINE class

#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS
PM_LINE::PM_LINE(PM_CCONST PM_LINE & l)
{
    start = l.start;
    end = l.end;
    uVec = l.uVec;
}
#endif

int PM_LINE::init(PM_POSE start, PM_POSE end)
{
    PmLine _line;
    PmPose _start, _end;
    int retval;

    toPose(start, &_start);
    toPose(end, &_end);

    retval = pmLineInit(&_line, _start, _end);

    toLine(_line, this);

    return retval;
}

int PM_LINE::point(double len, PM_POSE * point)
{
    PmLine _line;
    PmPose _point;
    int retval;

    toLine(*this, &_line);

    retval = pmLinePoint(&_line, len, &_point);

    toPose(_point, point);

    return retval;
}

// PM_CIRCLE class

#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS
PM_CIRCLE::PM_CIRCLE(PM_CCONST PM_CIRCLE & c)
{
    center = c.center;
    normal = c.normal;
    rTan = c.rTan;
    rPerp = c.rPerp;
    rHelix = c.rHelix;
    radius = c.radius;
    angle = c.angle;
    spiral = c.spiral;
}
#endif

int PM_CIRCLE::init(PM_POSE start, PM_POSE end,
    PM_CARTESIAN center, PM_CARTESIAN normal, int turn)
{
    PmCircle _circle;
    PmPose _start, _end;
    PmCartesian _center, _normal;
    int retval;

    toPose(start, &_start);
    toPose(end, &_end);
    toCart(center, &_center);
    toCart(normal, &_normal);

    retval = pmCircleInit(&_circle, _start, _end, _center, _normal, turn);

    toCircle(_circle, this);

    return retval;
}

int PM_CIRCLE::point(double angle, PM_POSE * point)
{
    PmCircle _circle;
    PmPose _point;
    int retval;

    toCircle(*this, &_circle);

    retval = pmCirclePoint(&_circle, angle, &_point);

    toPose(_point, point);

    return retval;
}

// overloaded external functions

// dot

double dot(PM_CARTESIAN v1, PM_CARTESIAN v2)
{
    double d;
    PmCartesian _v1, _v2;

    toCart(v1, &_v1);
    toCart(v2, &_v2);

    pmCartCartDot(_v1, _v2, &d);

    return d;
}

// cross

PM_CARTESIAN cross(PM_CARTESIAN v1, PM_CARTESIAN v2)
{
    PM_CARTESIAN ret;
    PmCartesian _v1, _v2;

    toCart(v1, &_v1);
    toCart(v2, &_v2);

    pmCartCartCross(_v1, _v2, &_v1);

    toCart(_v1, &ret);

    return ret;
}

//unit

PM_CARTESIAN unit(PM_CARTESIAN v)
{
    PM_CARTESIAN vout;
    PmCartesian _v;

    toCart(v, &_v);

    pmCartUnit(_v, &_v);

    toCart(_v, &vout);

    return vout;
}

/*! \todo Another #if 0 */
#if 0

PM_CARTESIAN norm(PM_CARTESIAN v)
{
    PM_CARTESIAN vout;
    PmCartesian _v;

    toCart(v, &_v);

    pmCartNorm(_v, &_v);

    toCart(_v, &vout);

    return vout;
}
#endif

PM_QUATERNION norm(PM_QUATERNION q)
{
    PM_QUATERNION qout;
    PmQuaternion _q;

    toQuat(q, &_q);
    pmQuatNorm(_q, &_q);

    toQuat(_q, &qout);

    return qout;
}

PM_ROTATION_VECTOR norm(PM_ROTATION_VECTOR r)
{
    PM_ROTATION_VECTOR rout;
    PmRotationVector _r;

    toRot(r, &_r);

    pmRotNorm(_r, &_r);

    toRot(_r, &rout);

    return rout;
}

PM_ROTATION_MATRIX norm(PM_ROTATION_MATRIX m)
{
    PM_ROTATION_MATRIX mout;
    PmRotationMatrix _m;

    toMat(m, &_m);

    pmMatNorm(_m, &_m);

    toMat(_m, &mout);

    return mout;
}

// isNorm

int isNorm(PM_CARTESIAN v)
{
    PmCartesian _v;

    toCart(v, &_v);

    return pmCartIsNorm(_v);
}

int isNorm(PM_QUATERNION q)
{
    PmQuaternion _q;

    toQuat(q, &_q);

    return pmQuatIsNorm(_q);
}

int isNorm(PM_ROTATION_VECTOR r)
{
    PmRotationVector _r;

    toRot(r, &_r);

    return pmRotIsNorm(_r);
}

int isNorm(PM_ROTATION_MATRIX m)
{
    PmRotationMatrix _m;

    toMat(m, &_m);

    return pmMatIsNorm(_m);
}

// mag

double mag(PM_CARTESIAN v)
{
    double ret;
    PmCartesian _v;

    toCart(v, &_v);

    pmCartMag(_v, &ret);

    return ret;
}

// disp

double disp(PM_CARTESIAN v1, PM_CARTESIAN v2)
{
    double ret;
    PmCartesian _v1, _v2;

    toCart(v1, &_v1);
    toCart(v2, &_v2);

    pmCartCartDisp(_v1, _v2, &ret);

    return ret;
}

// inv

PM_CARTESIAN inv(PM_CARTESIAN v)
{
    PM_CARTESIAN ret;
    PmCartesian _v;

    toCart(v, &_v);

    pmCartInv(_v, &_v);

    toCart(_v, &ret);

    return ret;
}

PM_ROTATION_MATRIX inv(PM_ROTATION_MATRIX m)
{
    PM_ROTATION_MATRIX ret;
    PmRotationMatrix _m;

    toMat(m, &_m);

    pmMatInv(_m, &_m);

    toMat(_m, &ret);

    return ret;
}

PM_QUATERNION inv(PM_QUATERNION q)
{
    PM_QUATERNION ret;
    PmQuaternion _q;

    toQuat(q, &_q);

    pmQuatInv(_q, &_q);

    toQuat(_q, &ret);

    return ret;
}

PM_POSE inv(PM_POSE p)
{
    PM_POSE ret;
    PmPose _p;

    toPose(p, &_p);

    pmPoseInv(_p, &_p);

    toPose(_p, &ret);

    return ret;
}

PM_HOMOGENEOUS inv(PM_HOMOGENEOUS h)
{
    PM_HOMOGENEOUS ret;
    PmHomogeneous _h;

    toHom(h, &_h);

    pmHomInv(_h, &_h);

    toHom(_h, &ret);

    return ret;
}

// project

PM_CARTESIAN proj(PM_CARTESIAN v1, PM_CARTESIAN v2)
{
    PM_CARTESIAN ret;
    PmCartesian _v1, _v2;

    toCart(v1, &_v1);
    toCart(v2, &_v2);

    pmCartCartProj(_v1, _v2, &_v1);

    toCart(_v1, &ret);

    return ret;
}

// overloaded arithmetic operators

PM_CARTESIAN operator +(PM_CARTESIAN v)
{
    return v;
}

PM_CARTESIAN operator -(PM_CARTESIAN v)
{
    PM_CARTESIAN ret;

    ret.x = -v.x;
    ret.y = -v.y;
    ret.z = -v.z;

    return ret;
}

PM_QUATERNION operator +(PM_QUATERNION q)
{
    return q;
}

PM_QUATERNION operator -(PM_QUATERNION q)
{
    PM_QUATERNION ret;
    PmQuaternion _q;

    toQuat(q, &_q);

    pmQuatInv(_q, &_q);

    toQuat(_q, &ret);

    return ret;
}

PM_POSE operator +(PM_POSE p)
{
    return p;
}

PM_POSE operator -(PM_POSE p)
{
    PM_POSE ret;
    PmPose _p;

    toPose(p, &_p);

    pmPoseInv(_p, &_p);

    toPose(_p, &ret);

    return ret;
}

int operator ==(PM_CARTESIAN v1, PM_CARTESIAN v2)
{
    PmCartesian _v1, _v2;

    toCart(v1, &_v1);
    toCart(v2, &_v2);

    return pmCartCartCompare(_v1, _v2);
}

int operator ==(PM_QUATERNION q1, PM_QUATERNION q2)
{
    PmQuaternion _q1, _q2;

    toQuat(q1, &_q1);
    toQuat(q2, &_q2);

    return pmQuatQuatCompare(_q1, _q2);
}

int operator ==(PM_POSE p1, PM_POSE p2)
{
    PmPose _p1, _p2;

    toPose(p1, &_p1);
    toPose(p2, &_p2);

    return pmPosePoseCompare(_p1, _p2);
}

int operator !=(PM_CARTESIAN v1, PM_CARTESIAN v2)
{
    PmCartesian _v1, _v2;

    toCart(v1, &_v1);
    toCart(v2, &_v2);

    return !pmCartCartCompare(_v1, _v2);
}

int operator !=(PM_QUATERNION q1, PM_QUATERNION q2)
{
    PmQuaternion _q1, _q2;

    toQuat(q1, &_q1);
    toQuat(q2, &_q2);

    return !pmQuatQuatCompare(_q1, _q2);
}

int operator !=(PM_POSE p1, PM_POSE p2)
{
    PmPose _p1, _p2;

    toPose(p1, &_p1);
    toPose(p2, &_p2);

    return !pmPosePoseCompare(_p1, _p2);
}

PM_CARTESIAN operator +(PM_CARTESIAN v1, PM_CARTESIAN v2)
{
    PM_CARTESIAN ret;

    ret.x = v1.x + v2.x;
    ret.y = v1.y + v2.y;
    ret.z = v1.z + v2.z;

    return ret;
}

PM_CARTESIAN operator -(PM_CARTESIAN v1, PM_CARTESIAN v2)
{
    PM_CARTESIAN ret;

    ret.x = v1.x - v2.x;
    ret.y = v1.y - v2.y;
    ret.z = v1.z - v2.z;

    return ret;
}

PM_CARTESIAN operator *(PM_CARTESIAN v, double s)
{
    PM_CARTESIAN ret;

    ret.x = v.x * s;
    ret.y = v.y * s;
    ret.z = v.z * s;

    return ret;
}

PM_CARTESIAN operator *(double s, PM_CARTESIAN v)
{
    PM_CARTESIAN ret;

    ret.x = v.x * s;
    ret.y = v.y * s;
    ret.z = v.z * s;

    return ret;
}

PM_CARTESIAN operator /(PM_CARTESIAN v, double s)
{
    PM_CARTESIAN ret;

#ifdef PM_DEBUG
    if (s == 0.0) {
#ifdef PM_PRINT_ERROR
	pmPrintError("PM_CARTESIAN::operator / : divide by 0\n");
#endif
	pmErrno = PM_DIV_ERR;
	return ret;
    }
#endif

    ret.x = v.x / s;
    ret.y = v.y / s;
    ret.z = v.z / s;

    return ret;
}

PM_QUATERNION operator *(double s, PM_QUATERNION q)
{
    PM_QUATERNION qout;
    PmQuaternion _q;

    toQuat(q, &_q);

    pmQuatScalMult(_q, s, &_q);

    toQuat(_q, &qout);

    return qout;
}

PM_QUATERNION operator *(PM_QUATERNION q, double s)
{
    PM_QUATERNION qout;
    PmQuaternion _q;

    toQuat(q, &_q);

    pmQuatScalMult(_q, s, &_q);

    toQuat(_q, &qout);

    return qout;
}

PM_QUATERNION operator /(PM_QUATERNION q, double s)
{
    PM_QUATERNION qout;
    PmQuaternion _q;

    toQuat(q, &_q);

#ifdef PM_DEBUG
    if (s == 0.0) {
#ifdef PM_PRINT_ERROR
	pmPrintError("Divide by 0 in operator /\n");
#endif
	pmErrno = PM_NORM_ERR;

/*! \todo Another #if 0 */
#if 0
	// g++/gcc versions 2.8.x and 2.9.x
	// will complain that the call to PM_QUATERNION(PM_QUATERNION) is
	// ambigous. (2.7.x and some others allow it)
	return qout =
	    PM_QUATERNION((double) 0, (double) 0, (double) 0, (double) 0);
#else

	PmQuaternion quat;

	quat.s = 0;
	quat.x = 0;
	quat.y = 0;
	quat.z = 0;

	pmQuatNorm(quat, &quat);

	qout.s = quat.s;
	qout.x = quat.x;
	qout.y = quat.y;
	qout.z = quat.z;
	return qout;
#endif

    }
#endif

    pmQuatScalMult(_q, 1.0 / s, &_q);
    toQuat(_q, &qout);

    pmErrno = 0;
    return qout;
}

PM_CARTESIAN operator *(PM_QUATERNION q, PM_CARTESIAN v)
{
    PM_CARTESIAN vout;
    PmQuaternion _q;
    PmCartesian _v;

    toQuat(q, &_q);
    toCart(v, &_v);

    pmQuatCartMult(_q, _v, &_v);

    toCart(_v, &vout);

    return vout;
}

PM_QUATERNION operator *(PM_QUATERNION q1, PM_QUATERNION q2)
{
    PM_QUATERNION ret;
    PmQuaternion _q1, _q2;

    toQuat(q1, &_q1);
    toQuat(q2, &_q2);

    pmQuatQuatMult(_q1, _q2, &_q1);

    toQuat(_q1, &ret);

    return ret;
}

PM_ROTATION_MATRIX operator *(PM_ROTATION_MATRIX m1, PM_ROTATION_MATRIX m2)
{
    PM_ROTATION_MATRIX ret;
    PmRotationMatrix _m1, _m2;

    toMat(m1, &_m1);
    toMat(m2, &_m2);

    pmMatMatMult(_m1, _m2, &_m1);

    toMat(_m1, &ret);

    return ret;
}

PM_POSE operator *(PM_POSE p1, PM_POSE p2)
{
    PM_POSE ret;
    PmPose _p1, _p2;

    toPose(p1, &_p1);
    toPose(p2, &_p2);

    pmPosePoseMult(_p1, _p2, &_p1);

    toPose(_p1, &ret);

    return ret;
}

PM_CARTESIAN operator *(PM_POSE p, PM_CARTESIAN v)
{
    PM_CARTESIAN ret;
    PmPose _p;
    PmCartesian _v;

    toPose(p, &_p);
    toCart(v, &_v);

    pmPoseCartMult(_p, _v, &_v);

    toCart(_v, &ret);

    return ret;
}