quadrilateralcowboy/cm/CollisionModel_rotate.cpp

/*
===========================================================================

Doom 3 GPL Source Code
Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. 

This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).  

Doom 3 Source Code is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

Doom 3 Source Code is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.

In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.

If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.

===========================================================================
*/

/*
===============================================================================

	Trace model vs. polygonal model collision detection.

===============================================================================
*/

#include "../idlib/precompiled.h"
#pragma hdrstop

#include "CollisionModel_local.h"

/*
===============================================================================

Collision detection for rotational motion

===============================================================================
*/

// epsilon for round-off errors in epsilon calculations
#define CM_PL_RANGE_EPSILON			1e-4f
// if the collision point is this close to the rotation axis it is not considered a collision
#define ROTATION_AXIS_EPSILON		(CM_CLIP_EPSILON*0.25f)


/*
================
CM_RotatePoint

  rotates a point about an arbitrary axis using the tangent of half the rotation angle
================
*/
void CM_RotatePoint( idVec3 &point, const idVec3 &origin, const idVec3 &axis, const float tanHalfAngle ) {
	double d, t, s, c;
	idVec3 proj, v1, v2;

	point -= origin;
	proj = axis * ( point * axis );
	v1 = point - proj;
	v2 = axis.Cross( v1 );

	// r = tan( a / 2 );
	// sin(a) = 2*r/(1+r*r);
	// cos(a) = (1-r*r)/(1+r*r);
	t = tanHalfAngle * tanHalfAngle;
	d = 1.0f / ( 1.0f + t );
	s = 2.0f * tanHalfAngle * d;
	c = ( 1.0f - t ) * d;

	point = v1 * c - v2 * s + proj + origin;
}

/*
================
CM_RotateEdge

  rotates an edge about an arbitrary axis using the tangent of half the rotation angle
================
*/
void CM_RotateEdge( idVec3 &start, idVec3 &end, const idVec3 &origin, const idVec3 &axis, const float tanHalfAngle ) {
	double d, t, s, c;
	idVec3 proj, v1, v2;

	// r = tan( a / 2 );
	// sin(a) = 2*r/(1+r*r);
	// cos(a) = (1-r*r)/(1+r*r);
	t = tanHalfAngle * tanHalfAngle;
	d = 1.0f / ( 1.0f + t );
	s = 2.0f * tanHalfAngle * d;
	c = ( 1.0f - t ) * d;

	start -= origin;
	proj = axis * ( start * axis );
	v1 = start - proj;
	v2 = axis.Cross( v1 );
	start = v1 * c - v2 * s + proj + origin;

	end -= origin;
	proj = axis * ( end * axis );
	v1 = end - proj;
	v2 = axis.Cross( v1 );
	end = v1 * c - v2 * s + proj + origin;
}

/*
================
idCollisionModelManagerLocal::CollisionBetweenEdgeBounds

  verifies if the collision of two edges occurs between the edge bounds
  also calculates the collision point and collision plane normal if the collision occurs between the bounds
================
*/
int idCollisionModelManagerLocal::CollisionBetweenEdgeBounds( cm_traceWork_t *tw, const idVec3 &va, const idVec3 &vb,
												   const idVec3 &vc, const idVec3 &vd, float tanHalfAngle,
												   idVec3 &collisionPoint, idVec3 &collisionNormal ) {
	float d1, d2, d;
	idVec3 at, bt, dir, dir1, dir2;
	idPluecker	pl1, pl2;

	at = va;
	bt = vb;
	if ( tanHalfAngle != 0.0f ) {
		CM_RotateEdge( at, bt, tw->origin, tw->axis, tanHalfAngle );
	}

	dir1 = (at - tw->origin).Cross( tw->axis );
	dir2 = (bt - tw->origin).Cross( tw->axis );
	if ( dir1 * dir1 > dir2 * dir2 ) {
		dir = dir1;
	}
	else {
		dir = dir2;
	}
	if ( tw->angle < 0.0f ) {
		dir = -dir;
	}

	pl1.FromLine( at, bt );
	pl2.FromRay( vc, dir );
	d1 = pl1.PermutedInnerProduct( pl2 );
	pl2.FromRay( vd, dir );
	d2 = pl1.PermutedInnerProduct( pl2 );
	if ( ( d1 > 0.0f && d2 > 0.0f ) || ( d1 < 0.0f && d2 < 0.0f ) ) {
		return false;
	}

	pl1.FromLine( vc, vd );
	pl2.FromRay( at, dir );
	d1 = pl1.PermutedInnerProduct( pl2 );
	pl2.FromRay( bt, dir );
	d2 = pl1.PermutedInnerProduct( pl2 );
	if ( ( d1 > 0.0f && d2 > 0.0f ) || ( d1 < 0.0f && d2 < 0.0f ) ) {
		return false;
	}

	// collision point on the edge at-bt
	dir1 = (vd - vc).Cross( dir );
	d = dir1 * vc;
	d1 = dir1 * at - d;
	d2 = dir1 * bt - d;
	if ( d1 == d2 ) {
		return false;
	}
	collisionPoint = at + ( d1 / (d1 - d2) ) * ( bt - at );

	// normal is cross product of the rotated edge va-vb and the edge vc-vd
	collisionNormal.Cross( bt-at, vd-vc );

	return true;
}

/*
================
idCollisionModelManagerLocal::RotateEdgeThroughEdge

  calculates the tangent of half the rotation angle at which the edges collide
================
*/
int idCollisionModelManagerLocal::RotateEdgeThroughEdge( cm_traceWork_t *tw, const idPluecker &pl1,
												const idVec3 &vc, const idVec3 &vd,
												const float minTan, float &tanHalfAngle ) {
	double v0, v1, v2, a, b, c, d, sqrtd, q, frac1, frac2;
	idVec3 ct, dt;
	idPluecker pl2;

	/*

	a = start of line being rotated
	b = end of line being rotated
	pl1 = pluecker coordinate for line (a - b)
	pl2 = pluecker coordinate for edge we might collide with (c - d)
	t = rotation angle around the z-axis
	solve pluecker inner product for t of rotating line a-b and line l2

	// start point of rotated line during rotation
	an[0] = a[0] * cos(t) + a[1] * sin(t)
	an[1] = a[0] * -sin(t) + a[1] * cos(t)
	an[2] = a[2];
	// end point of rotated line during rotation
	bn[0] = b[0] * cos(t) + b[1] * sin(t)
	bn[1] = b[0] * -sin(t) + b[1] * cos(t)
	bn[2] = b[2];

	pl1[0] = a[0] * b[1] - b[0] * a[1];
	pl1[1] = a[0] * b[2] - b[0] * a[2];
	pl1[2] = a[0] - b[0];
	pl1[3] = a[1] * b[2] - b[1] * a[2];
	pl1[4] = a[2] - b[2];
	pl1[5] = b[1] - a[1];

	v[0] = (a[0] * cos(t) + a[1] * sin(t)) * (b[0] * -sin(t) + b[1] * cos(t)) - (b[0] * cos(t) + b[1] * sin(t)) * (a[0] * -sin(t) + a[1] * cos(t));
	v[1] = (a[0] * cos(t) + a[1] * sin(t)) * b[2] - (b[0] * cos(t) + b[1] * sin(t)) * a[2];
	v[2] = (a[0] * cos(t) + a[1] * sin(t)) - (b[0] * cos(t) + b[1] * sin(t));
	v[3] = (a[0] * -sin(t) + a[1] * cos(t)) * b[2] - (b[0] * -sin(t) + b[1] * cos(t)) * a[2];
	v[4] = a[2] - b[2];
	v[5] = (b[0] * -sin(t) + b[1] * cos(t)) - (a[0] * -sin(t) + a[1] * cos(t));

	pl2[0] * v[4] + pl2[1] * v[5] + pl2[2] * v[3] + pl2[4] * v[0] + pl2[5] * v[1] + pl2[3] * v[2] = 0;

	v[0] = (a[0] * cos(t) + a[1] * sin(t)) * (b[0] * -sin(t) + b[1] * cos(t)) - (b[0] * cos(t) + b[1] * sin(t)) * (a[0] * -sin(t) + a[1] * cos(t));
	v[0] = (a[1] * b[1] - a[0] * b[0]) * cos(t) * sin(t) + (a[0] * b[1] + a[1] * b[0] * cos(t)^2) - (a[1] * b[0]) - ((b[1] * a[1] - b[0] * a[0]) * cos(t) * sin(t) + (b[0] * a[1] + b[1] * a[0]) * cos(t)^2 - (b[1] * a[0]))
	v[0] = - (a[1] * b[0]) - ( - (b[1] * a[0]))
	v[0] = (b[1] * a[0]) - (a[1] * b[0])

	v[0] = (a[0]*b[1]) - (a[1]*b[0]);
	v[1] = (a[0]*b[2] - b[0]*a[2]) * cos(t) + (a[1]*b[2] - b[1]*a[2]) * sin(t);
	v[2] = (a[0]-b[0]) * cos(t) + (a[1]-b[1]) * sin(t);
	v[3] = (b[0]*a[2] - a[0]*b[2]) * sin(t) + (a[1]*b[2] - b[1]*a[2]) * cos(t);
	v[4] = a[2] - b[2];
	v[5] = (a[0]-b[0]) * sin(t) + (b[1]-a[1]) * cos(t);

	v[0] = (a[0]*b[1]) - (a[1]*b[0]);
	v[1] = (a[0]*b[2] - b[0]*a[2]) * cos(t) + (a[1]*b[2] - b[1]*a[2]) * sin(t);
	v[2] = (a[0]-b[0]) * cos(t) - (b[1]-a[1]) * sin(t);
	v[3] = (a[0]*b[2] - b[0]*a[2]) * -sin(t) + (a[1]*b[2] - b[1]*a[2]) * cos(t);
	v[4] = a[2] - b[2];
	v[5] = (a[0]-b[0]) * sin(t) + (b[1]-a[1]) * cos(t);

	v[0] = pl1[0];
	v[1] = pl1[1] * cos(t) + pl1[3] * sin(t);
	v[2] = pl1[2] * cos(t) - pl1[5] * sin(t);
	v[3] = pl1[3] * cos(t) - pl1[1] * sin(t);
	v[4] = pl1[4];
	v[5] = pl1[5] * cos(t) + pl1[2] * sin(t);

	pl2[0] * v[4] + pl2[1] * v[5] + pl2[2] * v[3] + pl2[4] * v[0] + pl2[5] * v[1] + pl2[3] * v[2] = 0;

	0 =	pl2[0] * pl1[4] +
		pl2[1] * (pl1[5] * cos(t) + pl1[2] * sin(t)) +
		pl2[2] * (pl1[3] * cos(t) - pl1[1] * sin(t)) +
		pl2[4] * pl1[0] +
		pl2[5] * (pl1[1] * cos(t) + pl1[3] * sin(t)) +
		pl2[3] * (pl1[2] * cos(t) - pl1[5] * sin(t));

	v2 * cos(t) + v1 * sin(t) + v0 = 0;

	// rotation about the z-axis
	v0 = pl2[0] * pl1[4] + pl2[4] * pl1[0];
	v1 = pl2[1] * pl1[2] - pl2[2] * pl1[1] + pl2[5] * pl1[3] - pl2[3] * pl1[5];
	v2 = pl2[1] * pl1[5] + pl2[2] * pl1[3] + pl2[5] * pl1[1] + pl2[3] * pl1[2];

	// rotation about the x-axis
	//v0 = pl2[3] * pl1[2] + pl2[2] * pl1[3];
	//v1 = -pl2[5] * pl1[0] + pl2[4] * pl1[1] - pl2[1] * pl1[4] + pl2[0] * pl1[5];
	//v2 = pl2[4] * pl1[0] + pl2[5] * pl1[1] + pl2[0] * pl1[4] + pl2[1] * pl1[5];

	r = tan(t / 2);
	sin(t) = 2*r/(1+r*r);
	cos(t) = (1-r*r)/(1+r*r);

	v1 * 2 * r / (1 + r*r) + v2 * (1 - r*r) / (1 + r*r) + v0 = 0
	(v1 * 2 * r + v2 * (1 - r*r)) / (1 + r*r) = -v0
	(v1 * 2 * r + v2 - v2 * r*r) / (1 + r*r) = -v0
	v1 * 2 * r + v2 - v2 * r*r = -v0 * (1 + r*r)
	v1 * 2 * r + v2 - v2 * r*r = -v0 + -v0 * r*r
	(v0 - v2) * r * r + (2 * v1) * r + (v0 + v2) = 0;

	MrE gives Pluecker a banana.. good monkey

	*/

	tanHalfAngle = tw->maxTan;

	// transform rotation axis to z-axis
	ct = (vc - tw->origin) * tw->matrix;
	dt = (vd - tw->origin) * tw->matrix;

	pl2.FromLine( ct, dt );

	v0 = pl2[0] * pl1[4] + pl2[4] * pl1[0];
	v1 = pl2[1] * pl1[2] - pl2[2] * pl1[1] + pl2[5] * pl1[3] - pl2[3] * pl1[5];
	v2 = pl2[1] * pl1[5] + pl2[2] * pl1[3] + pl2[5] * pl1[1] + pl2[3] * pl1[2];

	a = v0 - v2;
	b = v1;
	c = v0 + v2;
	if ( a == 0.0f ) {
		if ( b == 0.0f ) {
			return false;
		}
		frac1 = -c / ( 2.0f * b );
		frac2 = 1e10;	// = tan( idMath::HALF_PI )
	}
	else {
		d = b * b - c * a;
		if ( d <= 0.0f ) {
			return false;
		}
		sqrtd = sqrt( d );
		if ( b > 0.0f ) {
			q = - b + sqrtd;
		}
		else {
			q = - b - sqrtd;
		}
		frac1 = q / a;
		frac2 = c / q;
	}

	if ( tw->angle < 0.0f ) {
		frac1 = -frac1;
		frac2 = -frac2;
	}

	// get smallest tangent for which a collision occurs
	if ( frac1 >= minTan && frac1 < tanHalfAngle ) {
		tanHalfAngle = frac1;
	}
	if ( frac2 >= minTan && frac2 < tanHalfAngle ) {
		tanHalfAngle = frac2;
	}

	if ( tw->angle < 0.0f ) {
		tanHalfAngle = -tanHalfAngle;
	}

	return true;
}

/*
================
idCollisionModelManagerLocal::EdgeFurthestFromEdge

  calculates the direction of motion at the initial position, where dir < 0 means the edges move towards each other
  if the edges move away from each other the tangent of half the rotation angle at which
  the edges are furthest apart is also calculated
================
*/
int idCollisionModelManagerLocal::EdgeFurthestFromEdge( cm_traceWork_t *tw, const idPluecker &pl1,
												const idVec3 &vc, const idVec3 &vd,
												float &tanHalfAngle, float &dir ) {
	double v0, v1, v2, a, b, c, d, sqrtd, q, frac1, frac2;
	idVec3 ct, dt;
	idPluecker pl2;

	/*

	v2 * cos(t) + v1 * sin(t) + v0 = 0;

	// rotation about the z-axis
	v0 = pl2[0] * pl1[4] + pl2[4] * pl1[0];
	v1 = pl2[1] * pl1[2] - pl2[2] * pl1[1] + pl2[5] * pl1[3] - pl2[3] * pl1[5];
	v2 = pl2[1] * pl1[5] + pl2[2] * pl1[3] + pl2[5] * pl1[1] + pl2[3] * pl1[2];

	derivative:
	v1 * cos(t) - v2 * sin(t) = 0;

	r = tan(t / 2);
	sin(t) = 2*r/(1+r*r);
	cos(t) = (1-r*r)/(1+r*r);

	-v2 * 2 * r / (1 + r*r) + v1 * (1 - r*r)/(1+r*r);
	-v2 * 2 * r + v1 * (1 - r*r) / (1 + r*r) = 0;
	-v2 * 2 * r + v1 * (1 - r*r) = 0;
	(-v1) * r * r + (-2 * v2) * r + (v1) = 0;

	*/

	tanHalfAngle = 0.0f;

	// transform rotation axis to z-axis
	ct = (vc - tw->origin) * tw->matrix;
	dt = (vd - tw->origin) * tw->matrix;

	pl2.FromLine( ct, dt );

	v0 = pl2[0] * pl1[4] + pl2[4] * pl1[0];
	v1 = pl2[1] * pl1[2] - pl2[2] * pl1[1] + pl2[5] * pl1[3] - pl2[3] * pl1[5];
	v2 = pl2[1] * pl1[5] + pl2[2] * pl1[3] + pl2[5] * pl1[1] + pl2[3] * pl1[2];

	// get the direction of motion at the initial position
	c = v0 + v2;
	if ( tw->angle > 0.0f ) {
		if ( c > 0.0f ) {
			dir = v1;
		}
		else {
			dir = -v1;
		}
	}
	else {
		if ( c > 0.0f ) {
			dir = -v1;
		}
		else {
			dir = v1;
		}
	}
	// negative direction means the edges move towards each other at the initial position
	if ( dir <= 0.0f ) {
		return true;
	}

	a = -v1;
	b = -v2;
	c = v1;
	if ( a == 0.0f ) {
		if ( b == 0.0f ) {
			return false;
		}
		frac1 = -c / ( 2.0f * b );
		frac2 = 1e10;	// = tan( idMath::HALF_PI )
	}
	else {
		d = b * b - c * a;
		if ( d <= 0.0f ) {
			return false;
		}
		sqrtd = sqrt( d );
		if ( b > 0.0f ) {
			q = - b + sqrtd;
		}
		else {
			q = - b - sqrtd;
		}
		frac1 = q / a;
		frac2 = c / q;
	}

	if ( tw->angle < 0.0f ) {
		frac1 = -frac1;
		frac2 = -frac2;
	}

	if ( frac1 < 0.0f && frac2 < 0.0f ) {
		return false;
	}

	if ( frac1 > frac2 ) {
		tanHalfAngle = frac1;
	}
	else {
		tanHalfAngle = frac2;
	}

	if ( tw->angle < 0.0f ) {
		tanHalfAngle = -tanHalfAngle;
	}

	return true;
}

/*
================
idCollisionModelManagerLocal::RotateTrmEdgeThroughPolygon
================
*/
void idCollisionModelManagerLocal::RotateTrmEdgeThroughPolygon( cm_traceWork_t *tw, cm_polygon_t *poly, cm_trmEdge_t *trmEdge ) {
	int i, j, edgeNum;
	float f1, f2, startTan, dir, tanHalfAngle;
	cm_edge_t *edge;
	cm_vertex_t *v1, *v2;
	idVec3 collisionPoint, collisionNormal, origin, epsDir;
	idPluecker epsPl;
	idBounds bounds;

	// if the trm is convex and the rotation axis intersects the trm
	if ( tw->isConvex && tw->axisIntersectsTrm ) {
		// if both points are behind the polygon the edge cannot collide within a 180 degrees rotation
		if ( tw->vertices[trmEdge->vertexNum[0]].polygonSide & tw->vertices[trmEdge->vertexNum[1]].polygonSide ) {
			return;
		}
	}

	// if the trace model edge rotation bounds do not intersect the polygon bounds
	if ( !trmEdge->rotationBounds.IntersectsBounds( poly->bounds ) ) {
		return;
	}

	// edge rotation bounds should cross polygon plane
	if ( trmEdge->rotationBounds.PlaneSide( poly->plane ) != SIDE_CROSS ) {
		return;
	}

	// check edges for a collision
	for ( i = 0; i < poly->numEdges; i++ ) {
		edgeNum = poly->edges[i];
		edge = tw->model->edges + abs(edgeNum);

		// if this edge is already checked
		if ( edge->checkcount == idCollisionModelManagerLocal::checkCount ) {
			continue;
		}

		// can never collide with internal edges
		if ( edge->internal ) {
			continue;
		}

		v1 = tw->model->vertices + edge->vertexNum[INTSIGNBITSET(edgeNum)];
		v2 = tw->model->vertices + edge->vertexNum[INTSIGNBITNOTSET(edgeNum)];

		// edge bounds
		for ( j = 0; j < 3; j++ ) {
			if ( v1->p[j] > v2->p[j] ) {
				bounds[0][j] = v2->p[j];
				bounds[1][j] = v1->p[j];
			}
			else {
				bounds[0][j] = v1->p[j];
				bounds[1][j] = v2->p[j];
			}
		}

		// if the trace model edge rotation bounds do not intersect the polygon edge bounds
		if ( !trmEdge->rotationBounds.IntersectsBounds( bounds ) ) {
			continue;
		}

		f1 = trmEdge->pl.PermutedInnerProduct( tw->polygonEdgePlueckerCache[i] );

		// pluecker coordinate for epsilon expanded edge
		epsDir = edge->normal * (CM_CLIP_EPSILON+CM_PL_RANGE_EPSILON);
		epsPl.FromLine( tw->model->vertices[edge->vertexNum[0]].p + epsDir,
						tw->model->vertices[edge->vertexNum[1]].p + epsDir );

		f2 = trmEdge->pl.PermutedInnerProduct( epsPl );

		// if the rotating edge is inbetween the polygon edge and the epsilon expanded edge
		if ( ( f1 < 0.0f && f2 > 0.0f ) || ( f1 > 0.0f && f2 < 0.0f ) ) {

			if ( !EdgeFurthestFromEdge( tw, trmEdge->plzaxis, v1->p, v2->p, startTan, dir ) ) {
				continue;
			}

			if ( dir <= 0.0f ) {
				// moving towards the polygon edge so stop immediately
				tanHalfAngle = 0.0f;
			}
			else if ( idMath::Fabs( startTan ) >= tw->maxTan ) {
				// never going to get beyond the start tangent during the current rotation
				continue;
			}
			else {
				// collide with the epsilon expanded edge
				if ( !RotateEdgeThroughEdge(tw, trmEdge->plzaxis, v1->p + epsDir, v2->p + epsDir, idMath::Fabs( startTan ), tanHalfAngle ) ) {
					tanHalfAngle = startTan;
				}
			}
		}
		else {
			// collide with the epsilon expanded edge
			epsDir = edge->normal * CM_CLIP_EPSILON;
			if ( !RotateEdgeThroughEdge(tw, trmEdge->plzaxis, v1->p + epsDir, v2->p + epsDir, 0.0f, tanHalfAngle ) ) {
				continue;
			}
		}

		if ( idMath::Fabs( tanHalfAngle ) >= tw->maxTan ) {
			continue;
		}

		// check if the collision is between the edge bounds
		if ( !CollisionBetweenEdgeBounds( tw, trmEdge->start, trmEdge->end, v1->p, v2->p,
												tanHalfAngle, collisionPoint, collisionNormal ) ) {
			continue;
		}

		// allow rotation if the rotation axis goes through the collisionPoint
		origin = tw->origin + tw->axis * ( tw->axis * ( collisionPoint - tw->origin ) );
		if ( ( collisionPoint - origin ).LengthSqr() < ROTATION_AXIS_EPSILON * ROTATION_AXIS_EPSILON ) {
			continue;
		}

		// fill in trace structure
		tw->maxTan = idMath::Fabs( tanHalfAngle );
		tw->trace.c.normal = collisionNormal;
		tw->trace.c.normal.Normalize();
		tw->trace.c.dist = tw->trace.c.normal * v1->p;
		// make sure the collision plane faces the trace model
		if ( (tw->trace.c.normal * trmEdge->start) - tw->trace.c.dist < 0 ) {
			tw->trace.c.normal = -tw->trace.c.normal;
			tw->trace.c.dist = -tw->trace.c.dist;
		}
		tw->trace.c.contents = poly->contents;
		tw->trace.c.material = poly->material;
		tw->trace.c.type = CONTACT_EDGE;
		tw->trace.c.modelFeature = edgeNum;
		tw->trace.c.trmFeature = trmEdge - tw->edges;
		tw->trace.c.point = collisionPoint;
		// if no collision can be closer
		if ( tw->maxTan == 0.0f ) {
			break;
		}
	}
}

/*
================
idCollisionModelManagerLocal::RotatePointThroughPlane

  calculates the tangent of half the rotation angle at which the point collides with the plane
================
*/
int idCollisionModelManagerLocal::RotatePointThroughPlane( const cm_traceWork_t *tw, const idVec3 &point, const idPlane &plane,
													const float angle, const float minTan, float &tanHalfAngle ) {
	double v0, v1, v2, a, b, c, d, sqrtd, q, frac1, frac2;
	idVec3 p, normal;

	/*

	p[0] = point[0] * cos(t) + point[1] * sin(t)
	p[1] = point[0] * -sin(t) + point[1] * cos(t)
	p[2] = point[2];

	normal[0] * (p[0] * cos(t) + p[1] * sin(t)) +
		normal[1] * (p[0] * -sin(t) + p[1] * cos(t)) +
			normal[2] * p[2] + dist = 0

	normal[0] * p[0] * cos(t) + normal[0] * p[1] * sin(t) +
		-normal[1] * p[0] * sin(t) + normal[1] * p[1] * cos(t) +
			normal[2] * p[2] + dist = 0

	v2 * cos(t) + v1 * sin(t) + v0

	// rotation about the z-axis
	v0 = normal[2] * p[2] + dist
	v1 = normal[0] * p[1] - normal[1] * p[0]
	v2 = normal[0] * p[0] + normal[1] * p[1]

	r = tan(t / 2);
	sin(t) = 2*r/(1+r*r);
	cos(t) = (1-r*r)/(1+r*r);

	v1 * 2 * r / (1 + r*r) + v2 * (1 - r*r) / (1 + r*r) + v0 = 0
	(v1 * 2 * r + v2 * (1 - r*r)) / (1 + r*r) = -v0
	(v1 * 2 * r + v2 - v2 * r*r) / (1 + r*r) = -v0
	v1 * 2 * r + v2 - v2 * r*r = -v0 * (1 + r*r)
	v1 * 2 * r + v2 - v2 * r*r = -v0 + -v0 * r*r
	(v0 - v2) * r * r + (2 * v1) * r + (v0 + v2) = 0;

	*/

	tanHalfAngle = tw->maxTan;

	// transform rotation axis to z-axis
	p = (point - tw->origin) * tw->matrix;
	d = plane[3] + plane.Normal() * tw->origin;
	normal = plane.Normal() * tw->matrix;

	v0 = normal[2] * p[2] + d;
	v1 = normal[0] * p[1] - normal[1] * p[0];
	v2 = normal[0] * p[0] + normal[1] * p[1];

	a = v0 - v2;
	b = v1;
	c = v0 + v2;
	if ( a == 0.0f ) {
		if ( b == 0.0f ) {
			return false;
		}
		frac1 = -c / ( 2.0f * b );
		frac2 = 1e10;	// = tan( idMath::HALF_PI )
	}
	else {
		d = b * b - c * a;
		if ( d <= 0.0f ) {
			return false;
		}
		sqrtd = sqrt( d );
		if ( b > 0.0f ) {
			q = - b + sqrtd;
		}
		else {
			q = - b - sqrtd;
		}
		frac1 = q / a;
		frac2 = c / q;
	}

	if ( angle < 0.0f ) {
		frac1 = -frac1;
		frac2 = -frac2;
	}

	// get smallest tangent for which a collision occurs
	if ( frac1 >= minTan && frac1 < tanHalfAngle ) {
		tanHalfAngle = frac1;
	}
	if ( frac2 >= minTan && frac2 < tanHalfAngle ) {
		tanHalfAngle = frac2;
	}

	if ( angle < 0.0f ) {
		tanHalfAngle = -tanHalfAngle;
	}

	return true;
}

/*
================
idCollisionModelManagerLocal::PointFurthestFromPlane

  calculates the direction of motion at the initial position, where dir < 0 means the point moves towards the plane
  if the point moves away from the plane the tangent of half the rotation angle at which
  the point is furthest away from the plane is also calculated
================
*/
int idCollisionModelManagerLocal::PointFurthestFromPlane( const cm_traceWork_t *tw, const idVec3 &point, const idPlane &plane,
													const float angle, float &tanHalfAngle, float &dir ) {

	double v1, v2, a, b, c, d, sqrtd, q, frac1, frac2;
	idVec3 p, normal;

	/*

	v2 * cos(t) + v1 * sin(t) + v0 = 0;

	// rotation about the z-axis
	v0 = normal[2] * p[2] + dist
	v1 = normal[0] * p[1] - normal[1] * p[0]
	v2 = normal[0] * p[0] + normal[1] * p[1]

	derivative:
	v1 * cos(t) - v2 * sin(t) = 0;

	r = tan(t / 2);
	sin(t) = 2*r/(1+r*r);
	cos(t) = (1-r*r)/(1+r*r);

	-v2 * 2 * r / (1 + r*r) + v1 * (1 - r*r)/(1+r*r);
	-v2 * 2 * r + v1 * (1 - r*r) / (1 + r*r) = 0;
	-v2 * 2 * r + v1 * (1 - r*r) = 0;
	(-v1) * r * r + (-2 * v2) * r + (v1) = 0;

	*/

	tanHalfAngle = 0.0f;

	// transform rotation axis to z-axis
	p = (point - tw->origin) * tw->matrix;
	normal = plane.Normal() * tw->matrix;

	v1 = normal[0] * p[1] - normal[1] * p[0];
	v2 = normal[0] * p[0] + normal[1] * p[1];

	// the point will always start at the front of the plane, therefore v0 + v2 > 0 is always true
	if ( angle < 0.0f ) {
		dir = -v1;
	}
	else {
		dir = v1;
	}
	// negative direction means the point moves towards the plane at the initial position
	if ( dir <= 0.0f ) {
		return true;
	}

	a = -v1;
	b = -v2;
	c = v1;
	if ( a == 0.0f ) {
		if ( b == 0.0f ) {
			return false;
		}
		frac1 = -c / ( 2.0f * b );
		frac2 = 1e10;	// = tan( idMath::HALF_PI )
	}
	else {
		d = b * b - c * a;
		if ( d <= 0.0f ) {
			return false;
		}
		sqrtd = sqrt( d );
		if ( b > 0.0f ) {
			q = - b + sqrtd;
		}
		else {
			q = - b - sqrtd;
		}
		frac1 = q / a;
		frac2 = c / q;
	}

	if ( angle < 0.0f ) {
		frac1 = -frac1;
		frac2 = -frac2;
	}

	if ( frac1 < 0.0f && frac2 < 0.0f ) {
		return false;
	}

	if ( frac1 > frac2 ) {
		tanHalfAngle = frac1;
	}
	else {
		tanHalfAngle = frac2;
	}

	if ( angle < 0.0f ) {
		tanHalfAngle = -tanHalfAngle;
	}

	return true;
}

/*
================
idCollisionModelManagerLocal::RotatePointThroughEpsilonPlane
================
*/
int idCollisionModelManagerLocal::RotatePointThroughEpsilonPlane( const cm_traceWork_t *tw, const idVec3 &point, const idVec3 &endPoint,
							const idPlane &plane, const float angle, const idVec3 &origin,
							float &tanHalfAngle, idVec3 &collisionPoint, idVec3 &endDir ) {
	float d, dir, startTan;
	idVec3 vec, startDir;
	idPlane epsPlane;

	// epsilon expanded plane
	epsPlane = plane;
	epsPlane.SetDist( epsPlane.Dist() + CM_CLIP_EPSILON );

	// if the rotation sphere at the rotation origin is too far away from the polygon plane
	d = epsPlane.Distance( origin );
	vec = point - origin;
	if ( d * d > vec * vec  ) {
		return false;
	}

	// calculate direction of motion at vertex start position
	startDir = ( point - origin ).Cross( tw->axis );
	if ( angle < 0.0f ) {
		startDir = -startDir;
	}
	// if moving away from plane at start position
	if ( startDir * epsPlane.Normal() >= 0.0f ) {
		// if end position is outside epsilon range
		d = epsPlane.Distance( endPoint );
		if ( d >= 0.0f ) {
			return false;	// no collision
		}
		// calculate direction of motion at vertex end position
		endDir = ( endPoint - origin ).Cross( tw->axis );
		if ( angle < 0.0f ) {
			endDir = -endDir;
		}
		// if also moving away from plane at end position
		if ( endDir * epsPlane.Normal() > 0.0f ) {
			return false; // no collision
		}
	}

	// if the start position is in the epsilon range
	d = epsPlane.Distance( point );
	if ( d <= CM_PL_RANGE_EPSILON ) {

		// calculate tangent of half the rotation for which the vertex is furthest away from the plane
		if ( !PointFurthestFromPlane( tw, point, plane, angle, startTan, dir ) ) {
			return false;
		}

		if ( dir <= 0.0f ) {
			// moving towards the polygon plane so stop immediately
			tanHalfAngle = 0.0f;
		}
		else if ( idMath::Fabs( startTan ) >= tw->maxTan ) {
			// never going to get beyond the start tangent during the current rotation
			return false;
		}
		else {
			// calculate collision with epsilon expanded plane
			if ( !RotatePointThroughPlane( tw, point, epsPlane, angle, idMath::Fabs( startTan ), tanHalfAngle ) ) {
				tanHalfAngle = startTan;
			}
		}
	}
	else {
		// calculate collision with epsilon expanded plane
		if ( !RotatePointThroughPlane( tw, point, epsPlane, angle, 0.0f, tanHalfAngle ) ) {
			return false;
		}
	}

	// calculate collision point
	collisionPoint = point;
	if ( tanHalfAngle != 0.0f ) {
		CM_RotatePoint( collisionPoint, tw->origin, tw->axis, tanHalfAngle );
	}
	// calculate direction of motion at collision point
	endDir = ( collisionPoint - origin ).Cross( tw->axis );
	if ( angle < 0.0f ) {
		endDir = -endDir;
	}
	return true;
}

/*
================
idCollisionModelManagerLocal::RotateTrmVertexThroughPolygon
================
*/
void idCollisionModelManagerLocal::RotateTrmVertexThroughPolygon( cm_traceWork_t *tw, cm_polygon_t *poly, cm_trmVertex_t *v, int vertexNum ) {
	int i;
	float tanHalfAngle;
	idVec3 endDir, collisionPoint;
	idPluecker pl;

	// if the trm vertex is behind the polygon plane it cannot collide with the polygon within a 180 degrees rotation
	if ( tw->isConvex && tw->axisIntersectsTrm && v->polygonSide ) {
		return;
	}

	// if the trace model vertex rotation bounds do not intersect the polygon bounds
	if ( !v->rotationBounds.IntersectsBounds( poly->bounds ) ) {
		return;
	}

	// vertex rotation bounds should cross polygon plane
	if ( v->rotationBounds.PlaneSide( poly->plane ) != SIDE_CROSS ) {
		return;
	}

	// rotate the vertex through the epsilon plane
	if ( !RotatePointThroughEpsilonPlane( tw, v->p, v->endp, poly->plane, tw->angle, v->rotationOrigin,
											tanHalfAngle, collisionPoint, endDir ) ) {
		return;
	}

	if ( idMath::Fabs( tanHalfAngle ) < tw->maxTan ) {
		// verify if 'collisionPoint' moving along 'endDir' moves between polygon edges
		pl.FromRay( collisionPoint, endDir );
		for ( i = 0; i < poly->numEdges; i++ ) {
			if ( poly->edges[i] < 0 ) {
				if ( pl.PermutedInnerProduct( tw->polygonEdgePlueckerCache[i] ) > 0.0f ) {
					return;
				}
			}
			else {
				if ( pl.PermutedInnerProduct( tw->polygonEdgePlueckerCache[i] ) < 0.0f ) {
					return;
				}
			}
		}
		tw->maxTan = idMath::Fabs( tanHalfAngle );
		// collision plane is the polygon plane
		tw->trace.c.normal = poly->plane.Normal();
		tw->trace.c.dist = poly->plane.Dist();
		tw->trace.c.contents = poly->contents;
		tw->trace.c.material = poly->material;
		tw->trace.c.type = CONTACT_TRMVERTEX;
		tw->trace.c.modelFeature = *reinterpret_cast<int *>(&poly);
		tw->trace.c.trmFeature = v - tw->vertices;
		tw->trace.c.point = collisionPoint;
	}
}

/*
================
idCollisionModelManagerLocal::RotateVertexThroughTrmPolygon
================
*/
void idCollisionModelManagerLocal::RotateVertexThroughTrmPolygon( cm_traceWork_t *tw, cm_trmPolygon_t *trmpoly, cm_polygon_t *poly, cm_vertex_t *v, idVec3 &rotationOrigin ) {
	int i, edgeNum;
	float tanHalfAngle;
	idVec3 dir, endp, endDir, collisionPoint;
	idPluecker pl;
	cm_trmEdge_t *edge;

	// if the polygon vertex is behind the trm plane it cannot collide with the trm polygon within a 180 degrees rotation
	if ( tw->isConvex && tw->axisIntersectsTrm && trmpoly->plane.Distance( v->p ) < 0.0f ) {
		return;
	}

	// if the model vertex is outside the trm polygon rotation bounds
	if ( !trmpoly->rotationBounds.ContainsPoint( v->p ) ) {
		return;
	}

	// if the rotation axis goes through the polygon vertex
	dir = v->p - rotationOrigin;
	if ( dir * dir < ROTATION_AXIS_EPSILON * ROTATION_AXIS_EPSILON ) {
		return;
	}

	// calculate vertex end position
	endp = v->p;
	tw->modelVertexRotation.RotatePoint( endp );

	// rotate the vertex through the epsilon plane
	if ( !RotatePointThroughEpsilonPlane( tw, v->p, endp, trmpoly->plane, -tw->angle, rotationOrigin,
											tanHalfAngle, collisionPoint, endDir ) ) {
		return;
	}

	if ( idMath::Fabs( tanHalfAngle ) < tw->maxTan ) {
		// verify if 'collisionPoint' moving along 'endDir' moves between polygon edges
		pl.FromRay( collisionPoint, endDir );
		for ( i = 0; i < trmpoly->numEdges; i++ ) {
			edgeNum = trmpoly->edges[i];
			edge = tw->edges + abs(edgeNum);
			if ( edgeNum < 0 ) {
				if ( pl.PermutedInnerProduct( edge->pl ) > 0.0f ) {
					return;
				}
			}
			else {
				if ( pl.PermutedInnerProduct( edge->pl ) < 0.0f ) {
					return;
				}
			}
		}
		tw->maxTan = idMath::Fabs( tanHalfAngle );
		// collision plane is the flipped trm polygon plane
		tw->trace.c.normal = -trmpoly->plane.Normal();
		tw->trace.c.dist = tw->trace.c.normal * v->p;
		tw->trace.c.contents = poly->contents;
		tw->trace.c.material = poly->material;
		tw->trace.c.type = CONTACT_MODELVERTEX;
		tw->trace.c.modelFeature = v - tw->model->vertices;
		tw->trace.c.trmFeature = trmpoly - tw->polys;
		tw->trace.c.point = v->p;
	}
}

/*
================
idCollisionModelManagerLocal::RotateTrmThroughPolygon

  returns true if the polygon blocks the complete rotation
================
*/
bool idCollisionModelManagerLocal::RotateTrmThroughPolygon( cm_traceWork_t *tw, cm_polygon_t *p ) {
	int i, j, k, edgeNum;
	float d;
	cm_trmVertex_t *bv;
	cm_trmEdge_t *be;
	cm_trmPolygon_t *bp;
	cm_vertex_t *v;
	cm_edge_t *e;
	idVec3 *rotationOrigin;

	// if already checked this polygon
	if ( p->checkcount == idCollisionModelManagerLocal::checkCount ) {
		return false;
	}
	p->checkcount = idCollisionModelManagerLocal::checkCount;

	// if this polygon does not have the right contents behind it
	if ( !(p->contents & tw->contents) ) {
		return false;
	}

	// if the the trace bounds do not intersect the polygon bounds
	if ( !tw->bounds.IntersectsBounds( p->bounds ) ) {
		return false;
	}

	// back face culling
	if ( tw->isConvex ) {
		// if the center of the convex trm is behind the polygon plane
		if ( p->plane.Distance( tw->start ) < 0.0f ) {
			// if the rotation axis intersects the trace model
			if ( tw->axisIntersectsTrm ) {
				return false;
			}
			else {
				// if the direction of motion at the start and end position of the
				// center of the trm both go towards or away from the polygon plane
				// or if the intersections of the rotation axis with the expanded heart planes
				// are both in front of the polygon plane
			}
		}
	}

	// if the polygon is too far from the first heart plane
	d = p->bounds.PlaneDistance( tw->heartPlane1 );
	if ( idMath::Fabs(d) > tw->maxDistFromHeartPlane1 ) {
		return false;
	}

	// rotation bounds should cross polygon plane
	switch( tw->bounds.PlaneSide( p->plane ) ) {
		case PLANESIDE_CROSS:
			break;
		case PLANESIDE_FRONT:
			if ( tw->model->isConvex ) {
				tw->quickExit = true;
				return true;
			}
		default:
			return false;
	}

	for ( i = 0; i < tw->numVerts; i++ ) {
		bv = tw->vertices + i;
		// calculate polygon side this vertex is on
		d = p->plane.Distance( bv->p );
		bv->polygonSide = FLOATSIGNBITSET( d );
	}

	for ( i = 0; i < p->numEdges; i++ ) {
		edgeNum = p->edges[i];
		e = tw->model->edges + abs(edgeNum);
		v = tw->model->vertices + e->vertexNum[INTSIGNBITSET(edgeNum)];

		// pluecker coordinate for edge
		tw->polygonEdgePlueckerCache[i].FromLine( tw->model->vertices[e->vertexNum[0]].p,
													tw->model->vertices[e->vertexNum[1]].p );

		// calculate rotation origin projected into rotation plane through the vertex
		tw->polygonRotationOriginCache[i] = tw->origin + tw->axis * ( tw->axis * ( v->p - tw->origin ) );
	}
	// copy first to last so we can easily cycle through
	tw->polygonRotationOriginCache[p->numEdges] = tw->polygonRotationOriginCache[0];

	// fast point rotation
	if ( tw->pointTrace ) {
		RotateTrmVertexThroughPolygon( tw, p, &tw->vertices[0], 0 );
	}
	else {
		// rotate trm vertices through polygon
		for ( i = 0; i < tw->numVerts; i++ ) {
			bv = tw->vertices + i;
			if ( bv->used ) {
				RotateTrmVertexThroughPolygon( tw, p, bv, i );
			}
		}

		// rotate trm edges through polygon
		for ( i = 1; i <= tw->numEdges; i++ ) {
			be = tw->edges + i;
			if ( be->used ) {
				RotateTrmEdgeThroughPolygon( tw, p, be );
			}
		}

		// rotate all polygon vertices through the trm
		for ( i = 0; i < p->numEdges; i++ ) {
			edgeNum = p->edges[i];
			e = tw->model->edges + abs(edgeNum);

			if ( e->checkcount == idCollisionModelManagerLocal::checkCount ) {
				continue;
			}
			// set edge check count
			e->checkcount = idCollisionModelManagerLocal::checkCount;
			// can never collide with internal edges
			if ( e->internal ) {
				continue;
			}
			// got to check both vertices because we skip internal edges
			for ( k = 0; k < 2; k++ ) {

				v = tw->model->vertices + e->vertexNum[k ^ INTSIGNBITSET(edgeNum)];

				// if this vertex is already checked
				if ( v->checkcount == idCollisionModelManagerLocal::checkCount ) {
					continue;
				}
				// set vertex check count
				v->checkcount = idCollisionModelManagerLocal::checkCount;

				// if the vertex is outside the trm rotation bounds
				if ( !tw->bounds.ContainsPoint( v->p ) ) {
					continue;
				}

				rotationOrigin = &tw->polygonRotationOriginCache[i+k];

				for ( j = 0; j < tw->numPolys; j++ ) {
					bp = tw->polys + j;
					if ( bp->used ) {
						RotateVertexThroughTrmPolygon( tw, bp, p, v, *rotationOrigin );
					}
				}
			}
		}
	}

	return ( tw->maxTan == 0.0f );
}

/*
================
idCollisionModelManagerLocal::BoundsForRotation

  only for rotations < 180 degrees
================
*/
void idCollisionModelManagerLocal::BoundsForRotation( const idVec3 &origin, const idVec3 &axis, const idVec3 &start, const idVec3 &end, idBounds &bounds ) {
	int i;
	float radiusSqr;
	idVec3 v1, v2;

	radiusSqr = ( start - origin ).LengthSqr();
	v1 = ( start - origin ).Cross( axis );
	v2 = ( end - origin ).Cross( axis );

	for ( i = 0; i < 3; i++ ) {
		// if the derivative changes sign along this axis during the rotation from start to end
		if ( ( v1[i] > 0.0f && v2[i] < 0.0f ) || ( v1[i] < 0.0f && v2[i] > 0.0f ) ) {
			if ( ( 0.5f * (start[i] + end[i]) - origin[i] ) > 0.0f ) {
				bounds[0][i] = Min( start[i], end[i] );
				bounds[1][i] = origin[i] + idMath::Sqrt( radiusSqr * ( 1.0f - axis[i] * axis[i] ) );
			}
			else {
				bounds[0][i] = origin[i] - idMath::Sqrt( radiusSqr * ( 1.0f - axis[i] * axis[i] ) );
				bounds[1][i] = Max( start[i], end[i] );
			}
		}
		else if ( start[i] > end[i] ) {
			bounds[0][i] = end[i];
			bounds[1][i] = start[i];
		}
		else {
			bounds[0][i] = start[i];
			bounds[1][i] = end[i];
		}
		// expand for epsilons
		bounds[0][i] -= CM_BOX_EPSILON;
		bounds[1][i] += CM_BOX_EPSILON;
	}
}

/*
================
idCollisionModelManagerLocal::Rotation180
================
*/
void idCollisionModelManagerLocal::Rotation180( trace_t *results, const idVec3 &rorg, const idVec3 &axis,
										const float startAngle, const float endAngle, const idVec3 &start,
										const idTraceModel *trm, const idMat3 &trmAxis, int contentMask,
										cmHandle_t model, const idVec3 &modelOrigin, const idMat3 &modelAxis ) {
	int i, j, edgeNum;
	float d, maxErr, initialTan;
	bool model_rotated, trm_rotated;
	idVec3 dir, dir1, dir2, tmp, vr, vup, org, at, bt;
	idMat3 invModelAxis, endAxis, tmpAxis;
	idRotation startRotation, endRotation;
	idPluecker plaxis;
	cm_trmPolygon_t *poly;
	cm_trmEdge_t *edge;
	cm_trmVertex_t *vert;
	ALIGN16( static cm_traceWork_t tw );

	if ( model < 0 || model > MAX_SUBMODELS || model > idCollisionModelManagerLocal::maxModels ) {
		common->Printf("idCollisionModelManagerLocal::Rotation180: invalid model handle\n");
		return;
	}
	if ( !idCollisionModelManagerLocal::models[model] ) {
		common->Printf("idCollisionModelManagerLocal::Rotation180: invalid model\n");
		return;
	}

	idCollisionModelManagerLocal::checkCount++;

	tw.trace.fraction = 1.0f;
	tw.trace.c.contents = 0;
	tw.trace.c.type = CONTACT_NONE;
	tw.contents = contentMask;
	tw.isConvex = true;
	tw.rotation = true;
	tw.positionTest = false;
	tw.axisIntersectsTrm = false;
	tw.quickExit = false;
	tw.angle = endAngle - startAngle;
	assert( tw.angle > -180.0f && tw.angle < 180.0f );
	tw.maxTan = initialTan = idMath::Fabs( tan( ( idMath::PI / 360.0f ) * tw.angle ) );
	tw.model = idCollisionModelManagerLocal::models[model];
	tw.start = start - modelOrigin;
	// rotation axis, axis is assumed to be normalized
	tw.axis = axis;
	assert( tw.axis[0] * tw.axis[0] + tw.axis[1] * tw.axis[1] + tw.axis[2] * tw.axis[2] > 0.99f );
	// rotation origin projected into rotation plane through tw.start
	tw.origin = rorg - modelOrigin;
	d = (tw.axis * tw.origin) - ( tw.axis * tw.start );
	tw.origin = tw.origin - d * tw.axis;
	// radius of rotation
	tw.radius = ( tw.start - tw.origin ).Length();
	// maximum error of the circle approximation traced through the axial BSP tree
	d = tw.radius * tw.radius - (CIRCLE_APPROXIMATION_LENGTH*CIRCLE_APPROXIMATION_LENGTH*0.25f);
	if ( d > 0.0f ) {
		maxErr = tw.radius - idMath::Sqrt( d );
	} else {
		maxErr = tw.radius;
	}

	model_rotated = modelAxis.IsRotated();
	if ( model_rotated ) {
		invModelAxis = modelAxis.Transpose();
		tw.axis *= invModelAxis;
		tw.origin *= invModelAxis;
	}

	startRotation.Set( tw.origin, tw.axis, startAngle );
	endRotation.Set( tw.origin, tw.axis, endAngle );

	// create matrix which rotates the rotation axis to the z-axis
	tw.axis.NormalVectors( vr, vup );
	tw.matrix[0][0] = vr[0];
	tw.matrix[1][0] = vr[1];
	tw.matrix[2][0] = vr[2];
	tw.matrix[0][1] = -vup[0];
	tw.matrix[1][1] = -vup[1];
	tw.matrix[2][1] = -vup[2];
	tw.matrix[0][2] = tw.axis[0];
	tw.matrix[1][2] = tw.axis[1];
	tw.matrix[2][2] = tw.axis[2];

	// if optimized point trace
	if ( !trm || ( trm->bounds[1][0] - trm->bounds[0][0] <= 0.0f &&
					trm->bounds[1][1] - trm->bounds[0][1] <= 0.0f &&
					trm->bounds[1][2] - trm->bounds[0][2] <= 0.0f ) ) {

		if ( model_rotated ) {
			// rotate trace instead of model
			tw.start *= invModelAxis;
		}
		tw.end = tw.start;
		// if we start at a specific angle
		if ( startAngle != 0.0f ) {
			startRotation.RotatePoint( tw.start );
		}
		// calculate end position of rotation
		endRotation.RotatePoint( tw.end );

		// calculate rotation origin projected into rotation plane through the vertex
		tw.numVerts = 1;
		tw.vertices[0].p = tw.start;
		tw.vertices[0].endp = tw.end;
		tw.vertices[0].used = true;
		tw.vertices[0].rotationOrigin = tw.origin + tw.axis * ( tw.axis * ( tw.vertices[0].p - tw.origin ) );
		BoundsForRotation( tw.vertices[0].rotationOrigin, tw.axis, tw.start, tw.end, tw.vertices[0].rotationBounds );
		// rotation bounds
		tw.bounds = tw.vertices[0].rotationBounds;
		tw.numEdges = tw.numPolys = 0;

		// collision with single point
		tw.pointTrace = true;

		// extents is set to maximum error of the circle approximation traced through the axial BSP tree
		tw.extents[0] = tw.extents[1] = tw.extents[2] = maxErr + CM_BOX_EPSILON;

		// setup rotation heart plane
		tw.heartPlane1.SetNormal( tw.axis );
		tw.heartPlane1.FitThroughPoint( tw.start );
		tw.maxDistFromHeartPlane1 = CM_BOX_EPSILON;

		// trace through the model
		idCollisionModelManagerLocal::TraceThroughModel( &tw );

		// store results
		*results = tw.trace;
		results->endpos = start;
		if ( tw.maxTan == initialTan ) {
			results->fraction = 1.0f;
		} else {
			results->fraction = idMath::Fabs( atan( tw.maxTan ) * ( 2.0f * 180.0f / idMath::PI ) / tw.angle );
		}
		assert( results->fraction <= 1.0f );
		endRotation.Set( rorg, axis, startAngle + (endAngle-startAngle) * results->fraction );
		endRotation.RotatePoint( results->endpos );
		results->endAxis.Identity();

		if ( results->fraction < 1.0f ) {
			// rotate trace plane normal if there was a collision with a rotated model
			if ( model_rotated ) {
				results->c.normal *= modelAxis;
				results->c.point *= modelAxis;
			}
			results->c.point += modelOrigin;
			results->c.dist += modelOrigin * results->c.normal;
		}
		return;
	}

	tw.pointTrace = false;

	// setup trm structure
	idCollisionModelManagerLocal::SetupTrm( &tw, trm );

	trm_rotated = trmAxis.IsRotated();

	// calculate vertex positions
	if ( trm_rotated ) {
		for ( i = 0; i < tw.numVerts; i++ ) {
			// rotate trm around the start position
			tw.vertices[i].p *= trmAxis;
		}
	}
	for ( i = 0; i < tw.numVerts; i++ ) {
		// set trm at start position
		tw.vertices[i].p += tw.start;
	}
	if ( model_rotated ) {
		for ( i = 0; i < tw.numVerts; i++ ) {
			tw.vertices[i].p *= invModelAxis;
		}
	}
	for ( i = 0; i < tw.numVerts; i++ ) {
		tw.vertices[i].endp = tw.vertices[i].p;
	}
	// if we start at a specific angle
	if ( startAngle != 0.0f ) {
		for ( i = 0; i < tw.numVerts; i++ ) {
			startRotation.RotatePoint( tw.vertices[i].p );
		}
	}
	for ( i = 0; i < tw.numVerts; i++ ) {
		// end position of vertex
		endRotation.RotatePoint( tw.vertices[i].endp );
	}

	// add offset to start point
	if ( trm_rotated ) {
		tw.start += trm->offset * trmAxis;
	} else {
		tw.start += trm->offset;
	}
	// if the model is rotated
	if ( model_rotated ) {
		// rotate trace instead of model
		tw.start *= invModelAxis;
	}
	tw.end = tw.start;
	// if we start at a specific angle
	if ( startAngle != 0.0f ) {
		startRotation.RotatePoint( tw.start );
	}
	// calculate end position of rotation
	endRotation.RotatePoint( tw.end );

	// setup trm vertices
	for ( vert = tw.vertices, i = 0; i < tw.numVerts; i++, vert++ ) {
		// calculate rotation origin projected into rotation plane through the vertex
		vert->rotationOrigin = tw.origin + tw.axis * ( tw.axis * ( vert->p - tw.origin ) );
		// calculate rotation bounds for this vertex
		BoundsForRotation( vert->rotationOrigin, tw.axis, vert->p, vert->endp, vert->rotationBounds );
		// if the rotation axis goes through the vertex then the vertex is not used
		d = ( vert->p - vert->rotationOrigin ).LengthSqr();
		if ( d > ROTATION_AXIS_EPSILON * ROTATION_AXIS_EPSILON ) {
			vert->used = true;
		}
	}

	// setup trm edges
	for ( edge = tw.edges + 1, i = 1; i <= tw.numEdges; i++, edge++ ) {
		// if the rotation axis goes through both the edge vertices then the edge is not used
		if ( tw.vertices[edge->vertexNum[0]].used | tw.vertices[edge->vertexNum[1]].used ) {
			edge->used = true;
		}
		// edge start, end and pluecker coordinate
		edge->start = tw.vertices[edge->vertexNum[0]].p;
		edge->end = tw.vertices[edge->vertexNum[1]].p;
		edge->pl.FromLine( edge->start, edge->end );
		// pluecker coordinate for edge being rotated about the z-axis
		at = ( edge->start - tw.origin ) * tw.matrix;
		bt = ( edge->end - tw.origin ) * tw.matrix;
		edge->plzaxis.FromLine( at, bt );
		// get edge rotation bounds from the rotation bounds of both vertices
		edge->rotationBounds = tw.vertices[edge->vertexNum[0]].rotationBounds;
		edge->rotationBounds.AddBounds( tw.vertices[edge->vertexNum[1]].rotationBounds );
		// used to calculate if the rotation axis intersects the trm
		edge->bitNum = 0;
	}

	tw.bounds.Clear();

	// rotate trm polygon planes
	if ( trm_rotated & model_rotated ) {
		tmpAxis = trmAxis * invModelAxis;
		for ( poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++ ) {
			poly->plane *= tmpAxis;
		}
	} else if ( trm_rotated ) {
		for ( poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++ ) {
			poly->plane *= trmAxis;
		}
	} else if ( model_rotated ) {
		for ( poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++ ) {
			poly->plane *= invModelAxis;
		}
	}

	// setup trm polygons
	for ( poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++ ) {
		poly->used = true;
		// set trm polygon plane distance
		poly->plane.FitThroughPoint( tw.edges[abs(poly->edges[0])].start );
		// get polygon bounds from edge bounds
		poly->rotationBounds.Clear();
		for ( j = 0; j < poly->numEdges; j++ ) {
			// add edge rotation bounds to polygon rotation bounds
			edge = &tw.edges[abs( poly->edges[j] )];
			poly->rotationBounds.AddBounds( edge->rotationBounds );
		}
		// get trace bounds from polygon bounds
		tw.bounds.AddBounds( poly->rotationBounds );
	}

	// extents including the maximum error of the circle approximation traced through the axial BSP tree
	for ( i = 0; i < 3; i++ ) {
		tw.size[0][i] = tw.bounds[0][i] - tw.start[i];
		tw.size[1][i] = tw.bounds[1][i] - tw.start[i];
		if ( idMath::Fabs( tw.size[0][i] ) > idMath::Fabs( tw.size[1][i] ) ) {
			tw.extents[i] = idMath::Fabs( tw.size[0][i] ) + maxErr + CM_BOX_EPSILON;
		} else {
			tw.extents[i] = idMath::Fabs( tw.size[1][i] ) + maxErr + CM_BOX_EPSILON;
		}
	}

	// for back-face culling
	if ( tw.isConvex ) {
		if ( tw.start == tw.origin ) {
			tw.axisIntersectsTrm = true;
		} else {
			// determine if the rotation axis intersects the trm
			plaxis.FromRay( tw.origin, tw.axis );
			for ( poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++ ) {
				// back face cull polygons
				if ( poly->plane.Normal() * tw.axis > 0.0f ) {
					continue;
				}
				// test if the axis goes between the polygon edges
				for ( j = 0; j < poly->numEdges; j++ ) {
					edgeNum = poly->edges[j];
					edge = tw.edges + abs(edgeNum);
					if ( !(edge->bitNum & 2) ) {
						d = plaxis.PermutedInnerProduct( edge->pl );
						edge->bitNum = FLOATSIGNBITSET( d ) | 2;
					}
					if ( ( edge->bitNum ^ INTSIGNBITSET( edgeNum ) ) & 1 ) {
						break;
					}
				}
				if ( j >= poly->numEdges ) {
					tw.axisIntersectsTrm = true;
					break;
				}
			}
		}
	}

	// setup rotation heart plane
	tw.heartPlane1.SetNormal( tw.axis );
	tw.heartPlane1.FitThroughPoint( tw.start );
	tw.maxDistFromHeartPlane1 = 0.0f;
	for ( i = 0; i < tw.numVerts; i++ ) {
		d = idMath::Fabs( tw.heartPlane1.Distance( tw.vertices[i].p ) );
		if ( d > tw.maxDistFromHeartPlane1 ) {
			tw.maxDistFromHeartPlane1 = d;
		}
	}
	tw.maxDistFromHeartPlane1 += CM_BOX_EPSILON;

	// inverse rotation to rotate model vertices towards trace model
	tw.modelVertexRotation.Set( tw.origin, tw.axis, -tw.angle );

	// trace through the model
	idCollisionModelManagerLocal::TraceThroughModel( &tw );

	// store results
	*results = tw.trace;
	results->endpos = start;
	if ( tw.maxTan == initialTan ) {
		results->fraction = 1.0f;
	} else {
		results->fraction = idMath::Fabs( atan( tw.maxTan ) * ( 2.0f * 180.0f / idMath::PI ) / tw.angle );
	}
	assert( results->fraction <= 1.0f );
	endRotation.Set( rorg, axis, startAngle + (endAngle-startAngle) * results->fraction );
	endRotation.RotatePoint( results->endpos );
	results->endAxis = trmAxis * endRotation.ToMat3();

	if ( results->fraction < 1.0f ) {
		// rotate trace plane normal if there was a collision with a rotated model
		if ( model_rotated ) {
			results->c.normal *= modelAxis;
			results->c.point *= modelAxis;
		}
		results->c.point += modelOrigin;
		results->c.dist += modelOrigin * results->c.normal;
	}
}

/*
================
idCollisionModelManagerLocal::Rotation
================
*/
#ifdef _DEBUG
static int entered = 0;
#endif

void idCollisionModelManagerLocal::Rotation( trace_t *results, const idVec3 &start, const idRotation &rotation,
										const idTraceModel *trm, const idMat3 &trmAxis, int contentMask,
										cmHandle_t model, const idVec3 &modelOrigin, const idMat3 &modelAxis ) {
	idVec3 tmp;
	float maxa, stepa, a, lasta;

	assert( ((byte *)&start) < ((byte *)results) || ((byte *)&start) > (((byte *)results) + sizeof( trace_t )) );
	assert( ((byte *)&trmAxis) < ((byte *)results) || ((byte *)&trmAxis) > (((byte *)results) + sizeof( trace_t )) );

	memset( results, 0, sizeof( *results ) );

	// if special position test
	if ( rotation.GetAngle() == 0.0f ) {
		idCollisionModelManagerLocal::ContentsTrm( results, start, trm, trmAxis, contentMask, model, modelOrigin, modelAxis );
		return;
	}

#ifdef _DEBUG
	bool startsolid = false;
	// test whether or not stuck to begin with
	if ( cm_debugCollision.GetBool() ) {
		if ( !entered ) {
			entered = 1;
			// if already messed up to begin with
			if ( idCollisionModelManagerLocal::Contents( start, trm, trmAxis, -1, model, modelOrigin, modelAxis ) & contentMask ) {
				startsolid = true;
			}
			entered = 0;
		}
	}
#endif

	if ( rotation.GetAngle() >= 180.0f || rotation.GetAngle() <= -180.0f) {
		if ( rotation.GetAngle() >= 360.0f ) {
			maxa = 360.0f;
			stepa = 120.0f;			// three steps strictly < 180 degrees
		} else if ( rotation.GetAngle() <= -360.0f ) {
			maxa = -360.0f;
			stepa = -120.0f;		// three steps strictly < 180 degrees
		} else {
			maxa = rotation.GetAngle();
			stepa = rotation.GetAngle() * 0.5f;	// two steps strictly < 180 degrees
		}
		for ( lasta = 0.0f, a = stepa; fabs( a ) < fabs( maxa ) + 1.0f; lasta = a, a += stepa ) {
			// partial rotation
			idCollisionModelManagerLocal::Rotation180( results, rotation.GetOrigin(), rotation.GetVec(), lasta, a, start, trm, trmAxis, contentMask, model, modelOrigin, modelAxis );
			// if there is a collision
			if ( results->fraction < 1.0f ) {
				// fraction of total rotation
				results->fraction = (lasta + stepa * results->fraction) / rotation.GetAngle();
				return;
			}
		}
		results->fraction = 1.0f;
		return;
	}

	idCollisionModelManagerLocal::Rotation180( results, rotation.GetOrigin(), rotation.GetVec(), 0.0f, rotation.GetAngle(), start, trm, trmAxis, contentMask, model, modelOrigin, modelAxis );

#ifdef _DEBUG
	// test for missed collisions
	if ( cm_debugCollision.GetBool() ) {
		if ( !entered ) {
			entered = 1;
			// if the trm is stuck in the model
			if ( idCollisionModelManagerLocal::Contents( results->endpos, trm, results->endAxis, -1, model, modelOrigin, modelAxis ) & contentMask ) {
				trace_t tr;

				// test where the trm is stuck in the model
				idCollisionModelManagerLocal::Contents( results->endpos, trm, results->endAxis, -1, model, modelOrigin, modelAxis );
				// re-run collision detection to find out where it failed
				idCollisionModelManagerLocal::Rotation( &tr, start, rotation, trm, trmAxis, contentMask, model, modelOrigin, modelAxis );
			}
			entered = 0;
		}
	}
#endif
}