dmtxvector2.c

Go to the documentation of this file.

 
#include <assert.h>
#include <math.h>
 
#include "dmtx.h"
#include "dmtxstatic.h"
 
extern DmtxVector2 *dmtxVector2AddTo(INOUT DmtxVector2 *v1, const DmtxVector2 *v2)
{
    v1->x += v2->x;
    v1->y += v2->y;
 
    return v1;
}
 
extern DmtxVector2 *dmtxVector2Add(OUT DmtxVector2 *vOut, const DmtxVector2 *v1, const DmtxVector2 *v2)
{
    *vOut = *v1;
 
    return dmtxVector2AddTo(vOut, v2);
}
 
extern DmtxVector2 *dmtxVector2SubFrom(INOUT DmtxVector2 *v1, const DmtxVector2 *v2)
{
    v1->x -= v2->x;
    v1->y -= v2->y;
 
    return v1;
}
 
extern DmtxVector2 *dmtxVector2Sub(OUT DmtxVector2 *vOut, const DmtxVector2 *v1, const DmtxVector2 *v2)
{
    *vOut = *v1;
 
    return dmtxVector2SubFrom(vOut, v2);
}
 
extern DmtxVector2 *dmtxVector2ScaleBy(INOUT DmtxVector2 *v, double s)
{
    v->x *= s;
    v->y *= s;
 
    return v;
}
 
extern DmtxVector2 *dmtxVector2Scale(OUT DmtxVector2 *vOut, const DmtxVector2 *v, double s)
{
    *vOut = *v;
 
    return dmtxVector2ScaleBy(vOut, s);
}
 
extern double dmtxVector2Cross(const DmtxVector2 *v1, const DmtxVector2 *v2)
{
    return (v1->x * v2->y) - (v1->y * v2->x);
}
 
extern double dmtxVector2Norm(INOUT DmtxVector2 *v)
{
    double mag;
 
    mag = dmtxVector2Mag(v);
 
    if (mag <= DmtxAlmostZero) {
        return -1.0; /* XXX this doesn't look clean */
    }
 
    dmtxVector2ScaleBy(v, 1 / mag);
 
    return mag;
}
 
extern double dmtxVector2Dot(const DmtxVector2 *v1, const DmtxVector2 *v2)
{
    return (v1->x * v2->x) + (v1->y * v2->y);
}
 
extern double dmtxVector2Mag(const DmtxVector2 *v)
{
    return sqrt(v->x * v->x + v->y * v->y);
}
 
extern double dmtxDistanceFromRay2(const DmtxRay2 *r, const DmtxVector2 *q)
{
    DmtxVector2 vSubTmp;
 
    double mag = dmtxVector2Mag(&(r->v));
    DmtxAssert(fabs(mag) > DmtxAlmostZero);
 
    return dmtxVector2Cross(&(r->v), dmtxVector2Sub(&vSubTmp, q, &(r->p))) / mag;
}
 
extern double dmtxDistanceAlongRay2(const DmtxRay2 *r, const DmtxVector2 *q)
{
    DmtxVector2 vSubTmp;
 
#ifdef DEBUG
    /* Assumes that v is a unit vector */
    if (fabs(1.0 - dmtxVector2Mag(&(r->v))) > DmtxAlmostZero) {
        ; /* XXX big error goes here */
    }
#endif
 
    return dmtxVector2Dot(dmtxVector2Sub(&vSubTmp, q, &(r->p)), &(r->v));
}
 
extern DmtxPassFail dmtxRay2Intersect(OUT DmtxVector2 *point, const DmtxRay2 *p0, const DmtxRay2 *p1)
{
    double numer, denom;
    DmtxVector2 w;
 
    denom = dmtxVector2Cross(&(p1->v), &(p0->v));
    if (fabs(denom) <= DmtxAlmostZero) {
        return DmtxFail;
    }
 
    dmtxVector2Sub(&w, &(p1->p), &(p0->p));
    numer = dmtxVector2Cross(&(p1->v), &w);
 
    return dmtxPointAlongRay2(point, p0, numer / denom);
}
 
extern DmtxPassFail dmtxPointAlongRay2(OUT DmtxVector2 *point, const DmtxRay2 *r, double t)
{
    DmtxVector2 vTmp;
 
    /* Ray should always have unit length of 1 */
    DmtxAssert(fabs(1.0 - dmtxVector2Mag(&(r->v))) <= DmtxAlmostZero);  // XXX: 是必须的吗？为什么不用dmtxVector2Norm
 
    dmtxVector2Scale(&vTmp, &(r->v), t);
    dmtxVector2Add(point, &(r->p), &vTmp);
 
    return DmtxPass;
}