//input vertex number is 2, constant
inline void computePlanarRotationMatrix(
const double *_Q, const int strideQ, const double *_Weight, const int strideW,
double3x3 &rot)
{
Vector3d& X = *((Vector3d *)&rot.x[0]);
Vector3d& Y = *((Vector3d *)&rot.x[3]);
Vector3d& Z = *((Vector3d *)&rot.x[6]);
Vector3d q1 = _getVertex3dUsingStride(_Q, 0, strideQ);
Vector3d q2 = _getVertex3dUsingStride(_Q, 1, strideQ);
const double w1 = _getDoubleUsingStride(_Weight, 0, strideW);
const double w2 = _getDoubleUsingStride(_Weight, 1, strideW);
Z = CrossProd(q1, q2);
const double l2 = Magnitude2(Z);
if (l2<1e-32){
X = q1; X.normalize();
const double nx = fabs(X.x);
const double ny = fabs(X.y);
const double nz = fabs(X.z);
const double maxnxyz = _MAX3_(nx, ny, nz);
if (nx==maxnxyz)
Z = Vector3d(0,0,1);
else if (ny==maxnxyz)
Z = Vector3d(0,0,1);
else
Z = Vector3d(-1,0,0);
Y = CrossProd(Z, X); Y.normalize();
Z = CrossProd(X, Y);
}
else{
Z *= 1.0/sqrt(l2); //normalize Z
q1.normalize(); q2.normalize();
Y = w1*q1+w2*q2; Y.normalize();
X = CrossProd(Y, Z);
}
}
/** Normalize vector. Return vector length. */
double Vec3::Normalize() {
double r = sqrt( Magnitude2() );
double b = 1.0 / r;
V_[0] *= b;
V_[1] *= b;
V_[2] *= b;
return r;
}
void StageCopyVA(VertexArray* to, VertexArray* from, float completion)
{
CopyVA(to, from);
float maxy = 0;
for(int i=0; i<to->numverts; i++)
if(to->vertices[i].y > maxy)
maxy = to->vertices[i].y;
float limity = maxy*completion;
for(int tri=0; tri<to->numverts/3; tri++)
{
Vec3f* belowv = NULL;
Vec3f* belowv2 = NULL;
for(int v=tri*3; v<tri*3+3; v++)
{
if(to->vertices[v].y <= limity)
{
if(!belowv)
belowv = &to->vertices[v];
else if(!belowv2)
belowv2 = &to->vertices[v];
//break;
}
}
Vec3f prevt[3];
prevt[0] = to->vertices[tri*3+0];
prevt[1] = to->vertices[tri*3+1];
prevt[2] = to->vertices[tri*3+2];
Vec2f prevc[3];
prevc[0] = to->texcoords[tri*3+0];
prevc[1] = to->texcoords[tri*3+1];
prevc[2] = to->texcoords[tri*3+2];
for(int v=tri*3; v<tri*3+3; v++)
{
if(to->vertices[v].y > limity)
{
float prevy = to->vertices[v].y;
to->vertices[v].y = limity;
#if 0
#if 0
void barycent(double x0, double y0, double z0, double x1, double y1, double z1, double x2, double y2, double z2,
double vx, double vy, double vz,
double *u, double *v, double *w)
#endif
double ratio0 = 0;
double ratio1 = 0;
double ratio2 = 0;
barycent(prevt[0].x, prevt[0].y, prevt[0].z,
prevt[1].x, prevt[1].y, prevt[1].z,
prevt[2].x, prevt[2].y, prevt[2].z,
to->vertices[v].x, to->vertices[v].y, to->vertices[v].z,
&ratio0, &ratio1, &ratio2);
to->texcoords[v].x = ratio0 * prevc[0].x + ratio1 * prevc[1].x + ratio2 * prevc[2].x;
to->texcoords[v].y = ratio0 * prevc[0].y + ratio1 * prevc[1].y + ratio2 * prevc[2].y;
#elif 0
Vec3f* closebelowv = NULL;
if(belowv)
closebelowv = belowv;
if(belowv2 && (!closebelowv || Magnitude2((*closebelowv) - to->vertices[v]) > Magnitude2((*belowv2) - to->vertices[v])))
closebelowv = belowv2;
float yratio = (closebelowv->y - prevy);
#endif
}
}
}
void MoveUnit(Unit* u)
{
UnitT* t = &g_utype[u->type];
u->prevpos = u->cmpos;
u->collided = false;
if(u->threadwait)
return;
if(Magnitude(u->goal - u->cmpos) <= t->cmspeed)
return;
if(u->underorder && u->target < 0 && Magnitude(u->goal - u->cmpos) <= PATHNODE_SIZE)
return;
if(u->path.size() <= 0 || *u->path.rbegin() != u->goal)
{
#if 1
if(t->military)
{
if(g_simframe - u->lastpath < u->pathdelay)
{
return;
}
//u->pathdelay += 1;
//u->pathdelay *= 2;
u->pathdelay += 10;
u->lastpath = g_simframe;
int nodesdist = Magnitude( u->goal - u->cmpos ) / PATHNODE_SIZE;
#if 1
PartialPath(u->type, u->mode,
u->cmpos.x, u->cmpos.y, u->target, u->target2, u->targtype, &u->path, &u->subgoal,
u, NULL, NULL,
u->goal.x, u->goal.y,
u->goal.x, u->goal.y, u->goal.x, u->goal.y,
nodesdist*10);
//TILE_SIZE*4/PATHNODE_SIZE);
#else
JPSPartPath(u->type, u->mode,
u->cmpos.x, u->cmpos.y, u->target, u->target2, u->targtype, &u->path, &u->subgoal,
u, NULL, NULL,
u->goal.x, u->goal.y,
u->goal.x, u->goal.y, u->goal.x, u->goal.y,
nodesdist*4);
#endif
#if 0
RichText rtext("ppathf");
NewTransx(u->drawpos + Vec3f(0,t->size.y,0), &rtext);
#endif
}
else if(!u->pathblocked)
#endif
{
#if 0
if(!FullPath(0,
u->type, u->mode,
u->cmpos.x, u->cmpos.y, u->target, u->target, u->target2, u->path, u->subgoal,
u, NULL, NULL,
u->goal.x, u->goal.y,
u->goal.x, u->goal.y, u->goal.x, u->goal.y))
#endif
JPSPath(
u->type, u->mode,
u->cmpos.x, u->cmpos.y, u->target, u->target2, u->targtype, &u->path, &u->subgoal,
u, NULL, NULL,
u->goal.x, u->goal.y,
u->goal.x, u->goal.y, u->goal.x, u->goal.y);
}
return;
}
u->freecollider();
Vec2i dir = u->subgoal - u->cmpos;
if(Magnitude2(u->subgoal - u->cmpos) <= t->cmspeed * t->cmspeed)
{
u->cmpos = u->subgoal;
if(u->path.size() >= 2)
{
u->path.erase( u->path.begin() );
u->subgoal = *u->path.begin();
dir = u->subgoal - u->cmpos;
}
#if 0
else
{
u->fillcollider();
u->drawpos.x = u->cmpos.x;
u->drawpos.z = u->cmpos.y;
u->drawpos.y = g_hmap.accheight(u->drawpos.x, u->drawpos.z);
u->rotation.y = GetYaw(dir.x, dir.y);
return;
}
#endif
}
if(dir.x != 0 || dir.y != 0)
{
u->rotation.y = GetYaw(dir.x, dir.y);
int mag = Magnitude(dir);
#if 0
if(mag <= 0)
mag = 1;
#endif
Vec2i scaleddir = dir * t->cmspeed / mag;
u->cmpos = u->cmpos + scaleddir;
#if 1
if(UnitCollides(u, u->cmpos, u->type))
#else
if(Trace(u->type, u->mode, u->prevpos, u->cmpos, u, NULL, NULL) != COLLIDER_NONE)
#endif
{
u->collided = true;
u->cmpos = u->prevpos;
u->path.clear();
u->subgoal = u->cmpos;
u->fillcollider();
return;
}
#if 0
u->collided = false;
#endif
}
if(UnitCollides(u, u->cmpos, u->type))
u->collided = true;
u->fillcollider();
u->drawpos.x = u->cmpos.x;
u->drawpos.z = u->cmpos.y;
u->drawpos.y = g_hmap.accheight(u->cmpos.x, u->cmpos.y);
}
double Vec3::Length() const { return sqrt( Magnitude2() ); }
void IdentifySuccessors_QP(PathJob* pj, PathNode* node)
{
Vec2i npos = PathNodePos(node);
int thisdistance = Magnitude2(Vec2i(npos.x - pj->ngoalx, npos.y - pj->ngoalz));
if( !pj->closestnode || thisdistance < pj->closest )
{
pj->closestnode = node;
pj->closest = thisdistance;
}
int runningD = 0;
if(node->previous)
runningD = node->previous->totalD;
bool standable[DIRS];
for(int i=0; i<DIRS; i++)
standable[i] = Standable(pj, npos.x + offsets[i].x, npos.y + offsets[i].y);
bool passable[DIRS];
passable[DIR_NW] = standable[DIR_NW] && standable[DIR_N] && standable[DIR_W];
passable[DIR_N] = standable[DIR_N];
passable[DIR_NE] = standable[DIR_NE] && standable[DIR_N] && standable[DIR_E];
passable[DIR_E] = standable[DIR_E];
passable[DIR_SE] = standable[DIR_SE] && standable[DIR_S] && standable[DIR_E];
passable[DIR_S] = standable[DIR_S];
passable[DIR_SW] = standable[DIR_SW] && standable[DIR_S] && standable[DIR_W];
passable[DIR_W] = standable[DIR_W];
for(int i=0; i<DIRS; i++)
{
if(!passable[i])
continue;
int newD = runningD + stepdist[i];
Vec2i nextnpos(npos.x + offsets[i].x, npos.y + offsets[i].y);
PathNode* nextn = PathNodeAt(nextnpos.x, nextnpos.y);
if(!nextn->closed && (!nextn->opened || newD < nextn->totalD))
{
g_toclear.push_back(nextn); // Records this node to reset its properties later.
nextn->totalD = newD;
int H = Manhattan( nextnpos - Vec2i(pj->ngoalx, pj->ngoalz) );
nextn->F = nextn->totalD + H;
nextn->previous = node;
if( !nextn->opened )
{
g_openlist.insert(nextn);
nextn->opened = true;
}
else
{
g_openlist.heapify(nextn);
}
}
}
}