C++ PerpendicularVector函数代码示例

/*
   ===============
   RotatePointAroundVector

   This is not implemented very well...
   ===============
 */
void RotatePointAroundVector( vec3_t dst, const vec3_t dir, const vec3_t point,
							  float degrees ) {
	float m[3][3];
	float im[3][3];
	float zrot[3][3];
	float tmpmat[3][3];
	float rot[3][3];
	int i;
	vec3_t vr, vup, vf;
	float rad;

	vf[0] = dir[0];
	vf[1] = dir[1];
	vf[2] = dir[2];

	PerpendicularVector( vr, dir );
	CrossProduct( vr, vf, vup );

	m[0][0] = vr[0];
	m[1][0] = vr[1];
	m[2][0] = vr[2];

	m[0][1] = vup[0];
	m[1][1] = vup[1];
	m[2][1] = vup[2];

	m[0][2] = vf[0];
	m[1][2] = vf[1];
	m[2][2] = vf[2];

	memcpy( im, m, sizeof( im ) );

	im[0][1] = m[1][0];
	im[0][2] = m[2][0];
	im[1][0] = m[0][1];
	im[1][2] = m[2][1];
	im[2][0] = m[0][2];
	im[2][1] = m[1][2];

	memset( zrot, 0, sizeof( zrot ) );
	zrot[0][0] = zrot[1][1] = zrot[2][2] = 1.0F;

	rad = (float)DEG2RAD( degrees );
	zrot[0][0] = (vec_t)cos( rad );
	zrot[0][1] = (vec_t)sin( rad );
	zrot[1][0] = (vec_t)-sin( rad );
	zrot[1][1] = (vec_t)cos( rad );

	MatrixMultiply( m, zrot, tmpmat );
	MatrixMultiply( tmpmat, im, rot );

	for ( i = 0; i < 3; i++ ) {
		dst[i] = rot[i][0] * point[0] + rot[i][1] * point[1] + rot[i][2] * point[2];
	}
}

void RotatePointAroundVector( vec3_t &dst, const vec3_t &dir, const vec3_t &point, float degrees )
{
	float	m[3][3];
	float	im[3][3];
	float	zrot[3][3];
	float	tmpmat[3][3];
	float	rot[3][3];
	int	i;
	vec3_t vr, vup, vf;

	vf[0] = dir[0];
	vf[1] = dir[1];
	vf[2] = dir[2];

	PerpendicularVector( vr, dir );
	//CrossProduct( vr, vf, vup );
	vup = CrossProduct( vr, vf );

	m[0][0] = vr[0];
	m[1][0] = vr[1];
	m[2][0] = vr[2];

	m[0][1] = vup[0];
	m[1][1] = vup[1];
	m[2][1] = vup[2];

	m[0][2] = vf[0];
	m[1][2] = vf[1];
	m[2][2] = vf[2];

	memcpy( im, m, sizeof( im ) );

	im[0][1] = m[1][0];
	im[0][2] = m[2][0];
	im[1][0] = m[0][1];
	im[1][2] = m[2][1];
	im[2][0] = m[0][2];
	im[2][1] = m[1][2];

	memset( zrot, 0, sizeof( zrot ) );
	zrot[0][0] = zrot[1][1] = zrot[2][2] = 1.0F;

	zrot[0][0] = cos( DEG2RAD( degrees ) );
	zrot[0][1] = sin( DEG2RAD( degrees ) );
	zrot[1][0] = -sin( DEG2RAD( degrees ) );
	zrot[1][1] = cos( DEG2RAD( degrees ) );

	R_ConcatRotations( m, zrot, tmpmat );
	R_ConcatRotations( tmpmat, im, rot );

	for ( i = 0; i < 3; i++ )
	{
		dst[i] = rot[i][0] * point[0] + rot[i][1] * point[1] + rot[i][2] * point[2];
	}
}

/**
 * @brief Rotate a point around a given vector
 * @param[in] dir The vector around which to rotate
 * @param[in] point The point to be rotated
 * @param[in] degrees How many degrees to rotate the point by
 * @param[out] dst The point after rotation
 * @note Warning: @c dst must be different from @c point (otherwise the result has no meaning)
 * @pre @c dir must be normalized
 */
void RotatePointAroundVector (vec3_t dst, const vec3_t dir, const vec3_t point, float degrees)
{
	float m[3][3];
	float im[3][3];
	float zrot[3][3];
	float tmpmat[3][3];
	float rot[3][3];
	int i;
	vec3_t vr, vup, vf;

	vf[0] = dir[0];
	vf[1] = dir[1];
	vf[2] = dir[2];

	PerpendicularVector(vr, dir);
	CrossProduct(vr, vf, vup);

	m[0][0] = vr[0];
	m[1][0] = vr[1];
	m[2][0] = vr[2];

	m[0][1] = vup[0];
	m[1][1] = vup[1];
	m[2][1] = vup[2];

	m[0][2] = vf[0];
	m[1][2] = vf[1];
	m[2][2] = vf[2];

	memcpy(im, m, sizeof(im));

	im[0][1] = m[1][0];
	im[0][2] = m[2][0];
	im[1][0] = m[0][1];
	im[1][2] = m[2][1];
	im[2][0] = m[0][2];
	im[2][1] = m[1][2];

	OBJZERO(zrot);

	/* now prepare the rotation matrix */
	zrot[0][0] = cos(degrees * torad);
	zrot[0][1] = sin(degrees * torad);
	zrot[1][0] = -sin(degrees * torad);
	zrot[1][1] = cos(degrees * torad);
	zrot[2][2] = 1.0F;

	R_ConcatRotations(m, zrot, tmpmat);
	R_ConcatRotations(tmpmat, im, rot);

	for (i = 0; i < 3; i++) {
		dst[i] = DotProduct(rot[i], point);
	}
}

/*
-----------------------------------------------------------------------------
 Function: RotatePointAroundVector -Rotate a point around a vector.

 Parameters: dst -[out] Point after rotation.
			 dir -[in] vector.
			 point -[in] Point.
			 degrees -[in] Degrees of rotation.

 Returns: Nothing.

 Notes: 
-----------------------------------------------------------------------------
*/
PUBLIC void RotatePointAroundVector( vec3_t dst, const vec3_t dir, const vec3_t point, float degrees ) 
{
	mat3_t	m;
	mat3_t	im;
	mat3_t	zrot;
	mat3_t	tmpmat;
	mat3_t	rot;
	vec3_t  vr, vup, vf;
	float	rad;

	vf[0] = dir[0];
	vf[1] = dir[1];
	vf[2] = dir[2];

	PerpendicularVector( vr, dir );
	vectorCrossProduct( vr, vf, vup );

	m[0] = vr[0];
	m[3] = vr[1];
	m[6] = vr[2];

	m[1] = vup[0];
	m[4] = vup[1];
	m[7] = vup[2];

	m[2] = vf[0];
	m[5] = vf[1];
	m[8] = vf[2];

	memcpy( im, m, sizeof( im ) );

	im[1] = m[3];
	im[2] = m[6];
	im[3] = m[1];
	im[5] = m[7];
	im[6] = m[2];
	im[7] = m[5];

	memset( zrot, 0, sizeof( zrot ) );
	zrot[0] = zrot[4] = zrot[8] = 1.0F;

	rad = DEG2RAD( degrees );
	zrot[0] = (float)cos( rad );
	zrot[1] = (float)sin( rad );
	zrot[3] = (float)-sin( rad );
	zrot[4] = (float)cos( rad );

	Matrix3x3Multiply( m, zrot, tmpmat );
	Matrix3x3Multiply( tmpmat, im, rot );

	dst[0] = rot[0] * point[0] + rot[1] * point[1] + rot[2] * point[2];
	dst[1] = rot[3] * point[0] + rot[4] * point[1] + rot[5] * point[2];
	dst[2] = rot[6] * point[0] + rot[7] * point[1] + rot[8] * point[2];	
}

static int vector_Perpendicular(lua_State * L)
{
	vec_t          *dst;
	vec_t          *src;

	dst = lua_getvector(L, 1);
	src = lua_getvector(L, 2);

	PerpendicularVector(dst, src);

	return 1;
}

/*
========================
GetSurfaceOrientations
========================
*/
void R_GetSurfaceOrientations( cplane_t *plane,  orientation_t *surface, orientation_t *camera )
{
	VectorCopy( plane->normal, surface->axis[0] );
	PerpendicularVector( surface->axis[1], surface->axis[0] );
	CrossProduct( surface->axis[0], surface->axis[1], surface->axis[2] );

	VectorScale( plane->normal, plane->dist, surface->origin );
	VectorCopy( surface->origin, camera->origin );
	VectorSubtract( vec3_origin, surface->axis[0], camera->axis[0] );
	VectorCopy( surface->axis[1], camera->axis[1] );
	VectorCopy( surface->axis[2], camera->axis[2] );

}

void RotatePointAroundVector( Coord *dst, Coord dir, Coord point, float degrees )
{
  float	m[3][3];
  float	im[3][3];
  float	zrot[3][3];
  float	tmpmat[3][3];
  float	rot[3][3];
  Coord vr, vup, vf;

  vf.x = dir.x;
  vf.y = dir.y;
  vf.z = dir.z;

  PerpendicularVector( &vr, dir );
  CrossProduct( vr, vf, &vup );
  m[0][0] = vr.x;
  m[1][0] = vr.y;
  m[2][0] = vr.z;

  m[0][1] = vup.x;
  m[1][1] = vup.y;
  m[2][1] = vup.z;

  m[0][2] = vf.x;
  m[1][2] = vf.y;
  m[2][2] = vf.z;

  memcpy( im, m, sizeof( im ) );

  im[0][1] = m[1][0];
  im[0][2] = m[2][0];
  im[1][0] = m[0][1];
  im[1][2] = m[2][1];
  im[2][0] = m[0][2];
  im[2][1] = m[1][2];

  memset( zrot, 0, sizeof( zrot ) );
  zrot[0][0] = zrot[1][1] = zrot[2][2] = 1.0F;

  zrot[0][0] = cos( ( degrees*DEGTORAD ) );
  zrot[0][1] = sin( ( degrees*DEGTORAD ) );
  zrot[1][0] = -sin( ( degrees*DEGTORAD ) );
  zrot[1][1] = cos( ( degrees*DEGTORAD ) );

  R_ConcatRotations( m, zrot, tmpmat );
  R_ConcatRotations( tmpmat, im, rot );

  dst->x = rot[0][0] * point.x + rot[0][1] * point.y + rot[0][2] * point.z;
  dst->y = rot[1][0] * point.x + rot[1][1] * point.y + rot[1][2] * point.z;
  dst->z = rot[2][0] * point.x + rot[2][1] * point.y + rot[2][2] * point.z;
}

void CG_EffectMark(qhandle_t markShader, const vec3_t origin, const vec3_t dir, float alpha, float radius)
{
	// 'quick' version of the CG_ImpactMark function

	vec3_t axis[3], originalPoints[4];
	float texCoordScale;
	byte colors[4];
	int i;
	polyVert_t *v, verts[4];

	if (!cg_addMarks.integer) {
		return;
	}

	if (radius <= 0) {
		CG_Error("CG_EffectMark called with <= 0 radius");
	}
	// create the texture axis
	VectorNormalize2(dir, axis[0]);
	PerpendicularVector(axis[1], axis[0]);
	VectorSet(axis[2], 1, 0, 0);	// This is _wrong_, but the function is for water anyway (i.e. usually flat)
	CrossProduct(axis[0], axis[2], axis[1]);

	texCoordScale = 0.5 * 1.0 / radius;

	// create the full polygon
	for (i = 0; i < 3; i++) {
		originalPoints[0][i] = origin[i] - radius * axis[1][i] - radius * axis[2][i];
		originalPoints[1][i] = origin[i] + radius * axis[1][i] - radius * axis[2][i];
		originalPoints[2][i] = origin[i] + radius * axis[1][i] + radius * axis[2][i];
		originalPoints[3][i] = origin[i] - radius * axis[1][i] + radius * axis[2][i];
	}

	colors[0] = 127;
	colors[1] = 127;
	colors[2] = 127;
	colors[3] = alpha * 255;

	for (i = 0, v = verts; i < 4; i++, v++) {
		vec3_t delta;

		VectorCopy(originalPoints[i], v->xyz);
		VectorSubtract(v->xyz, origin, delta);
		v->st[0] = 0.5 + DotProduct(delta, axis[1]) * texCoordScale;
		v->st[1] = 0.5 + DotProduct(delta, axis[2]) * texCoordScale;
		*(int *) v->modulate = *(int *) colors;
	}

	trap_R_AddPolyToScene(markShader, 4, verts);
}

// This is not implemented very well...
void RotatePointAroundVector( vector3 *dst, const vector3 *dir, const vector3 *point, float degrees ) {
	vector3 m[3], im[3], zrot[3], tmpmat[3], rot[3];
	vector3 vr, vup, vf;
	int	i;
	float	rad;

	vf.x = dir->x;
	vf.y = dir->y;
	vf.z = dir->z;

	PerpendicularVector( &vr, dir );
	CrossProduct( &vr, &vf, &vup );

	m[0].x = vr.x;
	m[1].x = vr.y;
	m[2].x = vr.z;

	m[0].y = vup.x;
	m[1].y = vup.y;
	m[2].y = vup.z;

	m[0].z = vf.x;
	m[1].z = vf.y;
	m[2].z = vf.z;

	memcpy( im, m, sizeof(im) );

	im[0].y = m[1].x;
	im[0].z = m[2].x;
	im[1].x = m[0].y;
	im[1].z = m[2].y;
	im[2].x = m[0].z;
	im[2].y = m[1].z;

	memset( zrot, 0, sizeof(zrot) );
	zrot[0].x = zrot[1].y = zrot[2].z = 1.0f;

	rad = DEG2RAD( degrees );
	zrot[0].x =  cosf( rad );
	zrot[0].y =  sinf( rad );
	zrot[1].x = -sinf( rad );
	zrot[1].y =  cosf( rad );

	MatrixMultiply( m, zrot, tmpmat );
	MatrixMultiply( tmpmat, im, rot );

	for ( i = 0; i < 3; i++ ) {
		dst->raw[i] = DotProduct( &rot[i], point );
	}
}

qboolean fire_weaponDir(gentity_t *self, vec3_t start, vec3_t dir, int weaponnum, float quadFactor, int handSide)
{
	vec3_t right, up;

	if (weaponnum <= 0 || weaponnum >= BG_NumWeapons()) {
		return qfalse;
	}

	// Get up and right from dir (which is "forward")
	PerpendicularVector( up, dir );
	CrossProduct( up, dir, right );

	return fire_projectile(self, start, dir, right, up, bg_weaponinfo[weaponnum].projnum,
		quadFactor, bg_weaponinfo[weaponnum].mod, bg_weaponinfo[weaponnum].splashMod, handSide);
}

// this should match CG_ShotgunPattern
void ShotgunPattern( vec3_t origin, vec3_t origin2, int seed, gentity_t *ent ) {
	int			i, n, c;
	float		r, u, d;
	vec3_t		end;
	vec3_t		forward, right, up;
	qboolean	hitClient = qfalse;

	// derive the right and up vectors from the forward vector, because
	// the client won't have any other information
	VectorNormalize2( origin2, forward );
	PerpendicularVector( right, forward );
	CrossProduct( forward, right, up );
	n = DEFAULT_SHOTGUN_COUNT;
	d = DEFAULT_SHOTGUN_SPREAD / 2;
	c = 3;
	while (n > 0) {
		float stepSize = (2 * M_PI) / c;
		float step;
		for (step = -M_PI; step < M_PI && n > 0; step += stepSize) {
			r = cos(step) * d;
			u = sin(step) * d;
			VectorMA( origin, 8192, forward, end);
			VectorMA (end, r, right, end);
			VectorMA (end, u, up, end);
			if( ShotgunPellet( origin, end, ent ) && !hitClient ) {
				hitClient = qtrue;
				ent->client->accuracy_hits++;
			}
			n--;
		}
		d += DEFAULT_SHOTGUN_SPREAD / 2;
		c += 4;
	}
	/*
	// generate the "random" spread pattern
	for ( i = 0 ; i < DEFAULT_SHOTGUN_COUNT ; i++ ) {
		r = Q_crandom( &seed ) * DEFAULT_SHOTGUN_SPREAD;  // CPM
		u = Q_crandom( &seed ) * DEFAULT_SHOTGUN_SPREAD;  // CPM
		VectorMA( origin, 8192, forward, end);
		VectorMA (end, r, right, end);
		VectorMA (end, u, up, end);
		if( ShotgunPellet( origin, end, ent ) && !hitClient ) {
			hitClient = qtrue;
			ent->client->accuracy_hits++;
		}
	}
	*/
}

void RotateAroundDirection( vector3 axis[3], float yaw ) {

	// create an arbitrary axis[1]
	PerpendicularVector( &axis[1], &axis[0] );

	// rotate it around axis[0] by yaw
	if ( yaw ) {
		vector3	temp;

		VectorCopy( &axis[1], &temp );
		RotatePointAroundVector( &axis[1], &axis[0], &temp, yaw );
	}

	// cross to get axis[2]
	CrossProduct( &axis[0], &axis[1], &axis[2] );
}

// this should match CG_ShotgunPattern
void ShotgunPattern( vec3_t origin, vec3_t origin2, int seed, gentity_t *ent ) {
	int			i;
	float		r, u;
	vec3_t		end;
	vec3_t		forward, right, up;
	qboolean	hitClient = qfalse;

//unlagged - attack prediction #2
	// use this for testing
	/*
	if ( g_unlagged.integer )
		Com_Printf( "Server seed: %d\n", seed );
		*/
//-unlagged - attack prediction #2

	// derive the right and up vectors from the forward vector, because
	// the client won't have any other information
	VectorNormalize2( origin2, forward );
	PerpendicularVector( right, forward );
	CrossProduct( forward, right, up );

//unlagged - backward reconciliation #2
	// backward-reconcile the other clients
	if ( g_unlagged.integer )
		G_DoTimeShiftFor( ent );
//-unlagged - backward reconciliation #2

	// generate the "random" spread pattern
	for ( i = 0 ; i < DEFAULT_SHOTGUN_COUNT ; i++ ) {
		r = Q_crandom( &seed ) * DEFAULT_SHOTGUN_SPREAD * 16;
		u = Q_crandom( &seed ) * DEFAULT_SHOTGUN_SPREAD * 16;
		VectorMA( origin, 8192 * 16, forward, end);
		VectorMA (end, r, right, end);
		VectorMA (end, u, up, end);
		if( ShotgunPellet( origin, end, ent ) && !hitClient ) {
			hitClient = qtrue;
			ent->client->accuracy_hits++;
		}
	}

//unlagged - backward reconciliation #2
	// put them back
	if ( g_unlagged.integer )
		G_UndoTimeShiftFor( ent );
//-unlagged - backward reconciliation #2
}

void Use_Shooter(gentity_t * ent, gentity_t * other, gentity_t * activator)
{
	vec3_t          dir;
	float           deg;
	vec3_t          up, right;

	// see if we have a target
	if(ent->enemy)
	{
		VectorSubtract(ent->enemy->r.currentOrigin, ent->s.origin, dir);
		VectorNormalize(dir);
	}
	else
	{
		VectorCopy(ent->movedir, dir);
	}

	// randomize a bit
	PerpendicularVector(up, dir);
	CrossProduct(up, dir, right);

	deg = crandom() * ent->random;
	VectorMA(dir, deg, up, dir);

	deg = crandom() * ent->random;
	VectorMA(dir, deg, right, dir);

	VectorNormalize(dir);

	switch (ent->s.weapon)
	{
		case WP_GRENADE_LAUNCHER:
			fire_grenade(ent, ent->s.origin, dir);
			break;
		case WP_ROCKET_LAUNCHER:
			fire_rocket(ent, ent->s.origin, dir);
			break;
		case WP_PLASMAGUN:
			fire_plasma(ent, ent->s.origin, dir);
			break;
	}

	G_AddEvent(ent, EV_FIRE_WEAPON, 0);
}

/*
** RB_DrawSun
*/
void RB_DrawSun( float scale, shader_t *shader ) {
	float		size;
	float		dist;
	vec3_t		origin, vec1, vec2;

	if ( !backEnd.skyRenderedThisView ) {
		return;
	}

	//qglLoadMatrixf( backEnd.viewParms.world.modelMatrix );
	//qglTranslatef (backEnd.viewParms.or.origin[0], backEnd.viewParms.or.origin[1], backEnd.viewParms.or.origin[2]);
	{
		// FIXME: this could be a lot cleaner
		mat4_t translation, modelview;

		Mat4Translation( backEnd.viewParms.or.origin, translation );
		Mat4Multiply( backEnd.viewParms.world.modelMatrix, translation, modelview );
		GL_SetModelviewMatrix( modelview );
	}

	dist = 	backEnd.viewParms.zFar / 1.75;		// div sqrt(3)
	size = dist * scale;

	VectorScale( tr.sunDirection, dist, origin );
	PerpendicularVector( vec1, tr.sunDirection );
	CrossProduct( tr.sunDirection, vec1, vec2 );

	VectorScale( vec1, size, vec1 );
	VectorScale( vec2, size, vec2 );

	// farthest depth range
	qglDepthRange( 1.0, 1.0 );

	RB_BeginSurface( shader, 0, 0 );

	RB_AddQuadStamp(origin, vec1, vec2, colorWhite);

	RB_EndSurface();

	// back to normal depth range
	qglDepthRange( 0.0, 1.0 );
}