Пример #1
0
//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);
	}
}
Пример #2
0
/** 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;
} 
Пример #3
0
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
			}
		}
	}
Пример #4
0
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);
}
Пример #5
0
double Vec3::Length() const { return sqrt( Magnitude2() ); }
Пример #6
0
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);
			}
		}
	}
}