/**
This interrupt services the ADC1/DMA3 current samples. The interrupt must
execute faster than the ADC and DMA channel can produce and transfer
#DMA_TRFRS samples into a DMA buffer. A good upper-bound is
cur_12bit_conversion_time_in_ns(). If the ADC clock period is #TAD_150NS
and the sampling time is #SAMP_12_TAD, then #DMA_TRFRS would take 16 * 150 *
(12 + 14) = 62.4 us. There is some overhead due to the DMA transfer, but
it's safe to stay under this limit. When the interrupt was timed last using
#TxTICK and #TxTOCK, it was executing in under 20 us (when branch is taken).
@warning this function assumes that there is only one pin being sampled for
current. If multiple pins are ever sampled, this function @b must be
updated.
@ingroup ints
*/
void GIBINT _DMA3Interrupt(void)
{
fractional c, offset;
// clear the interrupt right away so we can service next request
_DMA3IF = false;
// filter data
if(use_buf_a) {
c = VectorDotProduct(ADC_SAMPS, cur_buf_a[0], cur_filter);
}
else {
c = VectorDotProduct(ADC_SAMPS, cur_buf_b[0], cur_filter);
}
// update offset
if(is_motor_floating()) {
// take exponential moving average
fractional ema_data[EMA_LEN] = {c, my_current.offset};
my_current.offset =
VectorDotProduct(EMA_LEN, ema_data, offset_ema_filter);
}
offset = my_current.offset;
// update current
my_current.counts = c;
c = _Q15sub(c, offset);
my_current.mA = Fract2Float(c) * my_current.q15_to_mA;
use_buf_a ^= true;
}
float land_height(land_t *land,float *point) {
int i,j,k;
float x,y,dot0,dot1,p00[3],p10[3],p01[3],v10[3],v01[3],point0[3],point1[3],plane[4];
x = point[0] / land->step;
y = point[1] / land->step;
i = (int)x;
j = (int)y;
if(i < 0) i = 0;
else if(i > land->width - 2) i = land->width - 2;
if(j < 0) j = 0;
else if(j > land->height - 2) j = land->height - 2;
k = land->width * j + i;
if(x + y >= 1 + (int)x + (int)y) { // 1st or 2nd triangle
VectorCopy(land->vertex[k + land->width + 1].v,p00);
VectorCopy(land->vertex[k + land->width].v,p10);
VectorCopy(land->vertex[k + 1].v,p01);
} else {
VectorCopy(land->vertex[k].v,p00);
VectorCopy(land->vertex[k + 1].v,p10);
VectorCopy(land->vertex[k + land->width].v,p01);
}
VectorSub(p10,p00,v10);
VectorSub(p01,p00,v01);
VectorCrossProduct(v10,v01,plane);
plane[3] = -VectorDotProduct(plane,p00);
VectorCopy(point,point0);
VectorCopy(point,point1);
point0[2] = 0;
point1[2] = 1;
dot0 = -VectorDotProduct(plane,point0);
dot1 = -VectorDotProduct(plane,point1);
return (point0[2] + (point1[2] - point0[2]) *
(plane[3] - dot0) / (dot1 - dot0));
}
float heightground(const float *point) {
register int i,j;
float k,dot0,dot1,a[3],b[3],plane[4];
for(i = 0, j = 0; i < num_vertexground; i += 3, j += 24)
if(testpolygon(&vertexground[j],&vertexground[j + 8],&vertexground[j + 16],point)) break;
VectorCopy(&planeground[j / 6],plane);
plane[3] = planeground[j / 6 + 3];
VectorCopy(point,a);
VectorCopy(point,b);
b[2] += 1.0;
dot0 = -VectorDotProduct(plane,a);
dot1 = -VectorDotProduct(plane,b);
k = (plane[3] - dot0) / (dot1 - dot0);
return a[2] + (b[2] - a[2]) * k;
}
void land_render_process(land_node_t *node,camera_t *camera) {
int lod;
float dist,sub[3];
if(!node->left && !node->right) {
if(camera_check_box(camera,node->min,node->max)) {
VectorAdd(node->min,node->max,sub);
VectorScale(sub,0.5,sub);
VectorSub(sub,camera->pos,sub);
dist = sqrt(sub[0] * sub[0] + sub[1] * sub[1] + sub[2] * sub[2]);
if(dist < node->lod) lod = 0;
else lod = 1;
land_render_node(node,lod);
}
return;
}
if(-VectorDotProduct(camera->pos,node->plane) > node->plane[3]) {
land_render_process(node->left,camera);
if(camera_check_box(camera,node->right->min,node->right->max))
land_render_process(node->right,camera);
} else {
land_render_process(node->right,camera);
if(camera_check_box(camera,node->left->min,node->left->max))
land_render_process(node->left,camera);
}
}
void SimpleTransmissionGroups::EndUpdate(float infectivityCorrection)
{
// Distribute contagion shed to destination groups
float decay = 1.0f - contagionDecayRate;
int groupCount = scalingMatrix.size();
for (int iSink = 0; iSink < groupCount; iSink++)
{
MatrixRow_t& row = scalingMatrix[iSink];
float sum = VectorDotProduct(shedContagion, row);
sum *= infectivityCorrection;
currentContagion[iSink] *= decay;
currentContagion[iSink] += sum;
}
if (populationSize > 0)
{
memcpy(forceOfInfection.data(), currentContagion.data(), groupCount * sizeof(float));
VectorScalarMultiplyInPlace(forceOfInfection, 1.0f/populationSize);
}
else
{
memset(forceOfInfection.data(), 0, groupCount * sizeof(float));
}
memset(shedContagion.data(), 0, groupCount * sizeof(float));
}
float CEntityMng::CalculateRadius( vec3_t mins , vec3_t maxs )
{
float distMins , distMaxs , radius;
if( maxs[2] - mins[2] < m_viewSightMinHeight )
return -1.0f;
distMins = VectorDotProduct( mins , mins );
distMaxs = VectorDotProduct( maxs , maxs );
if( distMins > distMaxs )
radius = (float) sqrt( distMins );
else
radius = (float) sqrt( distMaxs );
return radius;
}
int land_create_div_plane(land_node_t *node) {
float point[3];
if(node->width <= LAND_NODE_SIZE && node->height <= LAND_NODE_SIZE) return 0;
VectorSet((node->min[0] + node->max[0]) / 2.0,(node->min[1] + node->max[1]) / 2.0,0,point);
VectorSet(0,0,0,node->plane);
if(node->width > node->height) node->plane[0] = 1;
else node->plane[1] = 1;
node->plane[3] = -VectorDotProduct(node->plane,point);
return 1;
}
inline int CItemMng::ChechVisibleDistance( vec3_t origin )
{
vec3_t dist;
VectorSubtract( dist , origin , m_camPos );
if( VectorDotProduct( dist , dist ) > m_visibleDist * m_visibleDist )
return false;
return true;
}
inline int CEntityMng::ChechEffectiveCamDistance( vec3_t origin , float effectiveDist )
{
vec3_t dist;
VectorSubtract( dist , origin , m_camPos );
if( VectorDotProduct( dist , dist ) > effectiveDist * effectiveDist )
return false;
return true;
}
extern bool IntersectionPointBettwenTwoSegments( const Point* beginSegmentA, const Point* endSegmentA,
const Point* beginSegmentB, const Point* endSegementB,
Point* intersectionPoint )
{
//如果有交点，则求交点并将交点插入正确的位置
//两相交线段求交点
//从而可以求出p的坐标 具体算法可以参考图形学算法
Vector segementA = {endSegmentA->x - beginSegmentA->x, 0.0f, endSegmentA->z - beginSegmentA->z};
Vector normalLineofSegementA;
VectorCrossProduct(&normalLineofSegementA, &segementA, &StandardVector);
Vector aBegin2BBegin = {beginSegmentB->x - beginSegmentA->x, 0.0f, beginSegmentB->z - beginSegmentA->z};
Vector aBegin2BEnd = {endSegementB->x - beginSegmentA->x, 0.0f, endSegementB->z - beginSegmentA->z};
float cosof2BeginPointB = VectorDotProduct(&aBegin2BBegin, &normalLineofSegementA);
float cosof2EndPointB = VectorDotProduct(&aBegin2BEnd, &normalLineofSegementA);
float tValue =cosof2BeginPointB;
tValue /= (cosof2BeginPointB - cosof2EndPointB);
if (tValue < 0.0 || tValue > 1.0)
{
return false;
}
float insertionX = (float)(beginSegmentB->x + tValue * (endSegementB->x - beginSegmentB->x));
float insertionZ = (float)(beginSegmentB->z + tValue * (endSegementB->z - beginSegmentB->z));
//检查点是否在线段的延长线上
Rect checkRect;
const Point point = {insertionX, insertionZ};
MakeRectByPoint(&checkRect, beginSegmentA, endSegmentA);
if (true != InRect(&checkRect, &point))
{
return false;
}
if(NULL != intersectionPoint)
{
*intersectionPoint = point;
}
return true;
}
int CEntityMng::ActivatePath( int characterID , char *start , char *dest , int playType )
{
activator_t *activator;
worldentity_t *startEnt , *destEnt , *nextEnt;
vec3_t diff;
float dist;
if( playType & GTH_ENTITY_PLAY_FORWARD )
{
startEnt = SearchTarget( start , GTH_WORLD_ENTITY_TYPE_PATH );
if( !startEnt ) return false;
destEnt = SearchTarget( dest , GTH_WORLD_ENTITY_TYPE_PATH );
nextEnt = startEnt->targetEntity;
}
else
{
startEnt = SearchTarget( dest , GTH_WORLD_ENTITY_TYPE_PATH );
if( !startEnt ) return false;
destEnt = SearchTarget( start , GTH_WORLD_ENTITY_TYPE_PATH );
nextEnt = startEnt->prevEntity;
}
if( nextEnt )
{
VectorSubtract( diff , nextEnt->origin , startEnt->origin );
dist = (float) sqrt( VectorDotProduct( diff , diff ) );
}
else
dist = 0.0f;
activator = startEnt->activator;
activator->startEntity = startEnt;
activator->destEntity = destEnt;
activator->currEntity = startEnt;
activator->playType = playType;
activator->startTime = m_timer->GetAppTime();
activator->currChainTime = activator->startTime;
activator->moveSpeed = GTH_ENTITY_PLAYER_MOVE_SPEED * startEnt->speed;
activator->currChainCompleteTime = dist / activator->moveSpeed;
activator->characterID = characterID;
if( m_currPathEntity )
Deactivate( m_currPathEntity );
activator->playing = true;
startEnt->inuse = true;
m_currPathEntity = startEnt;
return true;
}
int findcrosspoint(const float *a,const float *b,float *point) {
register int i,j,k;
float dot0,dot1,angle,p0[3],p1[3],p2[3];
for(i = 0, j = 0, k = 0; i < num_vertexground; i += 3, j += 4, k += 24) {
dot0 = -VectorDotProduct(a,&planeground[j]);
dot1 = -VectorDotProduct(b,&planeground[j]);
VectorSub(b,a,point);
VectorScale(point,(planeground[j + 3] - dot0) / (dot1 - dot0),point);
VectorAdd(point,a,point);
VectorSub(point,&vertexground[k],p0);
VectorSub(point,&vertexground[k + 8],p1);
VectorSub(point,&vertexground[k + 16],p2);
VectorNormalize(p0,p0);
VectorNormalize(p1,p1);
VectorNormalize(p2,p2);
angle = acos(VectorDotProduct(p0,p1)) +
acos(VectorDotProduct(p1,p2)) +
acos(VectorDotProduct(p2,p0));
if(angle > 6.28) return 1;
}
return 0;
}
void CEntityMng::SetPathState( activator_t *activator , worldentity_t *startEnt , worldentity_t *destEnt , float currRate )
{
vec3_t diff , origin , zero;
vec3_t diffAngles , angles;
Fx_CHARACTER_t *character;
float squareDist;
if( destEnt )
{
VectorSubtract( diff , destEnt->origin , startEnt->origin );
VectorSubtract( diffAngles , destEnt->angles , startEnt->angles );
}
else
{
VectorClear( diff );
VectorClear( diffAngles );
}
VectorMA( origin , startEnt->origin , diff , currRate );
VectorMA( angles , startEnt->angles , diffAngles , currRate );
if( activator->playType & GTH_ENTITY_PLAY_REVERSE )
{
angles[ PITCH ] = -angles[ PITCH ];
angles[ YAW ] += 180.0f;
}
character = &m_characterMng->m_Characters[ activator->characterID ];
VectorClear( zero );
m_move->Pmove( origin , zero , angles );
m_characterMng->UpdatePosition( activator->characterID , origin , angles );
squareDist = VectorDotProduct( diff , diff );
if( currRate == 0.0f || squareDist == 0.0f )
{
character->event = GTH_EV_CHAR_IDLE;
GTH_ChangeAnimation(character, ANIMTYPE_BYITEM, ANIM_ITEM_IDLE);
}
else if( startEnt->speed >= 1.5f )
{
character->event = GTH_EV_CHAR_RUN;
GTH_ChangeAnimation(character, ANIMTYPE_BYITEM, ANIM_ITEM_RUN);
}
else
{
character->event = GTH_EV_CHAR_WALK;
GTH_ChangeAnimation(character, ANIMTYPE_BYITEM, ANIM_ITEM_WALK);
}
}
void BuildQuaternion ( const vec3_t in_base,
const vec3_t in_vector,
vec4_t out_quat )
{
double theta;
double s, c;
const float error = 0.0001f;
if ( (in_vector [0] + 1.0f) < error )
{
out_quat [0] = 0.0f;
out_quat [1] = 1.0f;
out_quat [2] = 0.0f;
out_quat [3] = 0.0f;
}
else if ( (1.0f - in_vector [0]) < error )
{
out_quat [0] = 0.0f;
out_quat [1] = 0.0f;
out_quat [2] = 0.0f;
out_quat [3] = 1.0f;
}
else
{
VectorCrossProduct ( g_nVector, in_base, in_vector );
D3DXVec3Normalize ( (D3DXVECTOR3 *)g_nVector, (D3DXVECTOR3 *)g_nVector );
theta = acos ( VectorDotProduct ( in_base, in_vector ) / VectorLength ( in_vector ) );
s = sin ( theta / 2 );
c = cos ( theta / 2 );
out_quat [0] = (float)(s * g_nVector [0]);
out_quat [1] = (float)(s * g_nVector [1]);
out_quat [2] = (float)(s * g_nVector [2]);
out_quat [3] = (float)c;
D3DXQuaternionNormalize ( (D3DXQUATERNION *)out_quat, (D3DXQUATERNION *)out_quat );
}
return;
}
void GramSchmidtTranspose ( double * A, int n){
int i,j,k;
double a,max=0.;
NormalizeRows (A, n);
for ( i=0 ; i<n ; i++){
a = VectorNorm(&A[i*n],n);
for ( j=0 ; j<n ; j++)
A[i*n+j] /= a;
for ( j=i+1 ; j<n ; j++){
a = VectorDotProduct(&A[j*n],&A[i*n],n);
max = (max>fabs(a))?max:fabs(a);
for ( k=0 ; k<n ; k++)
A[j*n+k] -= a * A[i*n+k];
}
}
if(max>0.1)
printf ("Large change in GS\n");
}
int CEntityMng::LinkCameraPathChain( worldentity_t *entity , activator_t *activator )
{
worldentity_t *nextEnt;
vec3_t diff;
float dist;
if( activator->playType & GTH_ENTITY_PLAY_FORWARD )
nextEnt = entity->targetEntity;
else
nextEnt = entity->prevEntity;
if( !nextEnt || ( entity == activator->destEntity ) )
return false;
VectorSubtract( diff , nextEnt->origin , entity->origin );
dist = (float) sqrt( VectorDotProduct( diff , diff ) );
activator->currEntity = entity;
activator->currChainTime += activator->currChainCompleteTime;
activator->moveSpeed = GTH_ENTITY_CAMERA_MOVE_SPEED * entity->speed;
activator->currChainCompleteTime = dist / activator->moveSpeed;
return true;
}
inline int CEntityMng::ChechVisibleDistance( vec3_t origin , vec3_t mins , vec3_t maxs , float radius )
{
vec3_t diff;
float dist;
VectorSubtract( diff , origin , m_camPos );
dist = VectorDotProduct( diff , diff );
if( dist > m_removeDist * m_removeDist )
return ENTITY_OVER_THAN_VISIBLE;
if( radius <= 0.0f )
return ENTITY_OVER_THAN_CHARACTER;
if( dist < m_camDist * m_camDist )
return ENTITY_CLOSED_THAN_CHARACTER;
if( m_palyerPos[0] > ( origin[0] + mins[0] ) && m_palyerPos[0] < ( origin[0] + maxs[0] )
&& m_palyerPos[1] > ( origin[1] + mins[1] ) && m_palyerPos[1] < ( origin[1] + maxs[1] )
&& m_palyerPos[2] > ( origin[2] + mins[2] ) && m_palyerPos[2] < ( origin[2] + maxs[2] ) )
return ENTITY_CLOSED_THAN_CHARACTER;
return ENTITY_OVER_THAN_CHARACTER;
}
itementity_t* CItemMng::SearchCursorEntityNeo ( const vec3_t in_camPos,
const vec3_t in_camAngle,
matrix4x4_t *in_camTransMat,
const float in_minSquareDist,
const vec2_t in_mousePos,
const vec2_t in_viewportSize )
{
itementity_t *thisEntity = NULL;
itementity_t *firstCandidate = NULL;
itementity_t *thisCandidate = NULL;
itementity_t *closestCandidate = NULL;
bool isFirstCandidate = true;
vec3_t difference;
vec3_t screenPos;
bboxf_t bbox;
float halfWidth, halfHeight;
float red_ratio;
float diff;
float angle;
float minx, maxx, miny, maxy, minz, maxz;
float sqr_length;
float trival;
float ang_diff;
const float DISCARD_X = 100.0f;
const float DISCARD_Y = 100.0f;
m_minSquareDist = 100000000.0f;
thisEntity = m_linked;
while ( thisEntity )
{
if ( ! thisEntity->visible )
{
thisEntity = thisEntity->next;
continue;
}
VectorSubtract( difference, thisEntity->origin, in_camPos );
thisEntity->squareDist = VectorDotProduct( difference, difference );
if ( thisEntity->squareDist > m_minSquareDist )
{
thisEntity = thisEntity->next;
continue;
}
thisEntity->closerEntity = NULL;
thisEntity->fartherEntity = NULL;
if ( ! isFirstCandidate )
{
thisCandidate = firstCandidate;
if ( thisEntity->squareDist < firstCandidate->squareDist )
{
while ( 1 )
{
if ( thisCandidate->squareDist < thisEntity->squareDist )
{
if ( thisCandidate->fartherEntity )
{
thisEntity->fartherEntity = thisCandidate->fartherEntity;
thisCandidate->fartherEntity->closerEntity = thisEntity;
}
thisCandidate->fartherEntity = thisEntity;
thisEntity->closerEntity = thisCandidate;
break;
}
else if ( thisCandidate->closerEntity == NULL )
{
thisCandidate->closerEntity = thisEntity;
thisEntity->fartherEntity = thisCandidate;
closestCandidate = thisEntity;
break;
}
thisCandidate = thisCandidate->closerEntity;
}
}
else
{
while ( 1 )
{
if ( thisCandidate->squareDist > thisEntity->squareDist )
{
if ( thisCandidate->closerEntity )
{
thisEntity->closerEntity = thisCandidate->closerEntity;
thisCandidate->closerEntity->fartherEntity = thisEntity;
}
thisCandidate->closerEntity = thisEntity;
thisEntity->fartherEntity = thisCandidate;
break;
}
else if ( thisCandidate->fartherEntity == NULL )
{
thisCandidate->fartherEntity = thisEntity;
thisEntity->closerEntity = thisCandidate;
break;
}
thisCandidate = thisCandidate->fartherEntity;
}
}
}
else
{
firstCandidate = thisEntity;
closestCandidate = thisEntity;
isFirstCandidate = false;
}
thisEntity = thisEntity->next;
}
if ( closestCandidate == NULL )
return NULL;
halfWidth = (float)in_viewportSize[ 0 ] * 0.5f;
halfHeight = (float)in_viewportSize[ 1 ] * 0.5f;
float ratioFactor = 0.5f / (float)tan( g_camera.m_projectParm.fov / g_camera.m_projectParm.aspect * 0.5f * FX_DEGTORAD );
thisEntity = closestCandidate;
do
{
red_ratio = in_viewportSize[ 1 ] * ratioFactor / (float)sqrt( thisEntity->squareDist );
in_camTransMat->Transform( screenPos, thisEntity->origin );
screenPos[ 0 ] = ( 1.0f + screenPos[ 0 ] ) * halfWidth;
screenPos[ 1 ] = ( 1.0f - screenPos[ 1 ] ) * halfHeight;
diff = fabs( in_mousePos[ 1 ] - screenPos[ 1 ] );
if ( diff > DISCARD_Y * red_ratio ) goto SKIP;
diff = fabs( in_mousePos[ 0 ] - screenPos[ 0 ] );
if ( diff > DISCARD_X * red_ratio ) goto SKIP;
VectorCopy( &bbox[0], thisEntity->mins );
VectorCopy( &bbox[3], thisEntity->maxs );
diff *= diff;
ang_diff = in_camAngle[ YAW ] - ( thisEntity->angles[ YAW ] - 90.0f );
if ( ang_diff < 0.0f )
ang_diff += 360.0f;
minx = fabs( bbox[ 0 ] );
maxx = fabs( bbox[ 3 ] );
miny = fabs( bbox[ 1 ] );
maxy = fabs( bbox[ 4 ] );
if ( ( ang_diff >= 0.0f ) &&
( ang_diff < 90.0f ) )
{
if ( in_mousePos[ 0 ] < screenPos[ 0 ] )
{
sqr_length = maxx*maxx + miny*miny;
angle = (float)atan2( miny, maxx ) + ang_diff * FX_DEGTORAD;
trival = (float)fabs( sin( angle ) ) * red_ratio;
if ( diff > ( sqr_length * trival * trival ) )
goto SKIP;
}
else
{
sqr_length = minx*minx + maxy*maxy;
angle = (float)atan2( maxy, minx ) + ang_diff * FX_DEGTORAD;
trival = (float)fabs( sin( angle ) ) * red_ratio;
if ( diff > ( sqr_length * trival * trival ) )
goto SKIP;
}
}
else if ( ( ang_diff >= 90.0f ) &&
( ang_diff < 180.0f ) )
{
if ( in_mousePos[ 0 ] < screenPos[ 0 ] )
{
sqr_length = maxx*maxx + maxy*maxy;
angle = (float)atan2( maxy, maxx ) - ang_diff * FX_DEGTORAD;
trival = (float)fabs( sin( angle ) ) * red_ratio;
if ( diff > ( sqr_length * trival * trival ) )
goto SKIP;
}
else
{
sqr_length = minx*minx + miny*miny;
angle = (float)atan2( miny, minx ) - ang_diff * FX_DEGTORAD;
trival = (float)fabs( sin( angle ) ) * red_ratio;
if ( diff > ( sqr_length * trival * trival ) )
goto SKIP;
}
}
else if ( ( ang_diff >= 180.0f ) &&
( ang_diff < 270.0f ) )
{
if ( in_mousePos[ 0 ] < screenPos[ 0 ] )
{
sqr_length = minx*minx + maxy*maxy;
angle = (float)atan2( maxy, minx ) + ang_diff * FX_DEGTORAD;
trival = (float)fabs( sin( angle ) ) * red_ratio;
if ( diff > ( sqr_length * trival * trival ) )
goto SKIP;
}
else
{
sqr_length = maxx*maxx + miny*miny;
angle = (float)atan2( miny, maxx ) + ang_diff * FX_DEGTORAD;
trival = (float)fabs( sin( angle ) ) * red_ratio;
if ( diff > ( sqr_length * trival * trival ) )
goto SKIP;
}
}
else
{
if ( in_mousePos[ 0 ] < screenPos[ 0 ] )
{
sqr_length = minx*minx + miny*miny;
angle = (float)atan2( miny, minx ) - ang_diff * FX_DEGTORAD;
trival = (float)fabs( sin( angle ) ) * red_ratio;
if ( diff > ( sqr_length * trival * trival ) )
goto SKIP;
}
else
{
sqr_length = maxx*maxx + maxy*maxy;
angle = (float)atan2( maxy, maxx ) - ang_diff * FX_DEGTORAD;
trival = (float)fabs( sin( angle ) ) * red_ratio;
if ( diff > ( sqr_length * trival * trival ) )
goto SKIP;
}
}
diff = fabs( in_mousePos[ 1 ] - screenPos[ 1 ] );
diff *= diff;
ang_diff = -in_camAngle[ PITCH ];
if ( ang_diff < 0.0f )
ang_diff += 180.0f;
minz = fabs( bbox[ 2 ] );
maxz = fabs( bbox[ 5 ] );
if ( ( in_camAngle[ PITCH ] >= -90.0f ) &&
( in_camAngle[ PITCH ] < 0.0f ) )
{
if ( in_mousePos[ 1 ] < screenPos[ 1 ] )
{
sqr_length = maxy*maxy + maxz*maxz;
angle = (float)atan2( maxz, maxy ) + ang_diff * FX_DEGTORAD;
trival = (float)sin( angle ) * red_ratio;
if ( diff > ( sqr_length * trival * trival ) )
goto SKIP;
}
else
{
sqr_length = minz*minz + miny*miny;
angle = (float)atan2( minz, miny ) + ang_diff * FX_DEGTORAD;
trival = (float)sin( angle ) * red_ratio;
if ( diff > ( sqr_length * trival * trival ) )
goto SKIP;
}
}
else
{
if ( in_mousePos[ 1 ] < screenPos[ 1 ] )
{
sqr_length = miny*miny + maxz*maxz;
angle = (float)atan2( maxz, miny ) - ang_diff * FX_DEGTORAD;
trival = (float)sin( angle ) * red_ratio;
if ( diff > ( sqr_length * trival * trival ) )
goto SKIP;
}
else
{
sqr_length = minz*minz + maxy*maxy;
angle = (float)atan2( minz, maxy ) - ang_diff * FX_DEGTORAD;
trival = (float)sin( angle ) * red_ratio;
if ( diff > ( sqr_length * trival * trival ) )
goto SKIP;
}
}
m_minSquareDist = thisEntity->squareDist;
return thisEntity;
SKIP:
thisEntity = thisEntity->fartherEntity;
} while ( thisEntity != NULL );
return NULL;
}
int ConvexPolygonClipping(const MeshPolygon* clippedPolygon,
const MeshPolygon* clippingWindow,
Queue* queue)
{
if(NULL == clippedPolygon || NULL == clippingWindow || NULL == queue)
{
return WRONG_PARAM;
}
int allPointCountForClipping = 0;
const Point* pointListForClipingWindow = GetPolygonPointList(clippingWindow);
const int pointCountForClippingWindow = GetPolygonPointCount(clippingWindow);
allPointCountForClipping += pointCountForClippingWindow;
int clipingWindowPointInClippedWindowCount[3] = {0};
for (int i = 0; i < pointCountForClippingWindow; ++i)
{
int checkResult = PointInPolygon(clippedPolygon, pointListForClipingWindow[i].x, pointListForClipingWindow[i].z);
++clipingWindowPointInClippedWindowCount[checkResult + 1];
}
//裁剪多边形的顶点都位于被裁剪多边形的内部或者边上
//目前无法裁剪这种情况
if (0 == clipingWindowPointInClippedWindowCount[0])
{
return CLIPPING_WINDOW_ALL_IN_CLIPPED_POLYGON;
}
const Point* pointListForClippedPolygon = GetPolygonPointList(clippedPolygon);
const int pointCountForClippedPolygon = GetPolygonPointCount(clippedPolygon);
int clippedPolygonointInClippedWindowCount[3] = {0};
for (int i = 0; i < pointCountForClippedPolygon; ++i)
{
int checkResult = PointInPolygon(clippingWindow, pointListForClippedPolygon[i].x, pointListForClippedPolygon[i].z);
++clippedPolygonointInClippedWindowCount[checkResult + 1];
}
allPointCountForClipping += pointCountForClippedPolygon;
//被裁剪的多边形的顶点全部为与裁剪多边形内或边上
//不需要参见
if (0 == clippedPolygonointInClippedWindowCount[0])
{
return ALL_POINT_IN_CLIPPING_WINDOW;
}
//两多边形不相交 不需要裁剪
if (0 == clipingWindowPointInClippedWindowCount[2] && 0 == clippedPolygonointInClippedWindowCount[2])
{
return NOT_NEED_CLIPPING;
}
ClippingInterPoint* pointChainForClippingWindow = CreateClippingWindowData(clippedPolygon, clippingWindow);
ClippingInterPoint* pointChainForClippedPolygon = CreateClippedPolygonData(clippedPolygon, clippingWindow);
//查找交点并将交点插入正确的位置
const Vector* const pNormalOfClippedPolygon = GetPolygonNormal(clippedPolygon);
Rect checkRect;
for (int indexOfClippingWindow = 0; indexOfClippingWindow < pointCountForClippingWindow; ++indexOfClippingWindow)
{
int nextIndexOfClippingWindow = indexOfClippingWindow + 1 == pointCountForClippingWindow ? 0 : indexOfClippingWindow + 1;
for (int indexOfclippedPolygon = 0; indexOfclippedPolygon < pointCountForClippedPolygon; ++indexOfclippedPolygon)
{
int nextIndexOfClippedPolygon = indexOfclippedPolygon + 1 == pointCountForClippedPolygon ? 0 : indexOfclippedPolygon + 1;
ClippingInterPoint* const pBeginPointOfClippinWindow = &pointChainForClippingWindow[indexOfClippingWindow];
ClippingInterPoint* const pEndPointOfClippinWindow = &pointChainForClippingWindow[nextIndexOfClippingWindow];
ClippingInterPoint* const pBeginPointOfClippedPolygon = &pointChainForClippedPolygon[indexOfclippedPolygon];
ClippingInterPoint* const pEndPointOfClippedPolygon = &pointChainForClippedPolygon[nextIndexOfClippedPolygon];
Vector toBeginPoint = {pBeginPointOfClippinWindow->point.x - pBeginPointOfClippedPolygon->point.x,
0.0f,
pBeginPointOfClippinWindow->point.z - pBeginPointOfClippedPolygon->point.z};
Vector toEndPoint = {pEndPointOfClippinWindow->point.x - pBeginPointOfClippedPolygon->point.x,
0.0f,
pEndPointOfClippinWindow->point.z - pBeginPointOfClippedPolygon->point.z};
float cosofToBeginPoint = VectorDotProduct(&toBeginPoint, &pNormalOfClippedPolygon[indexOfclippedPolygon]);
float cosofToEndPoint = VectorDotProduct(&toEndPoint, &pNormalOfClippedPolygon[indexOfclippedPolygon]);
bool beginPointIsSpecial = (FloatEqualZero(cosofToBeginPoint) && cosofToEndPoint > 0.0f);
bool endPointIsSpecial = (FloatEqualZero(cosofToEndPoint) && cosofToBeginPoint> 0.0f);
if (beginPointIsSpecial || endPointIsSpecial)
{
//特殊点，即交点在被裁减的多边形的边上，且同在内侧
ClippingInterPoint* pTempPoint = beginPointIsSpecial ? pBeginPointOfClippinWindow : pEndPointOfClippinWindow;
pTempPoint->pointType = INTER_POINT_TYPE_CLIPPING_WINDOW_POINT_AN_INSERCTION_POINT;
ClippingInterPoint* pCurrentPositon = &pointChainForClippedPolygon[indexOfclippedPolygon];
ClippingInterPoint* pNextPosition = pCurrentPositon->next2ClippedPolygon;
ClippingInterPoint* pEndPosition = &pointChainForClippedPolygon[nextIndexOfClippedPolygon];
pTempPoint->tValueForClippedPolygon = GetTValue(&pBeginPointOfClippedPolygon->point, &pEndPointOfClippedPolygon->point, &pTempPoint->point);
while (true)
{
if( pTempPoint->tValueForClippedPolygon < pCurrentPositon->tValueForClippedPolygon)
{
ClippingInterPoint* pLastPosition = pCurrentPositon->prev2ClippedPolygon;
pLastPosition->next2ClippedPolygon = pTempPoint;
pCurrentPositon->prev2ClippedPolygon = pTempPoint;
pTempPoint->prev2ClippedPolygon = pLastPosition;
pTempPoint->next2ClippedPolygon = pCurrentPositon;
break;
}
else if (pEndPosition == pNextPosition)
{
pCurrentPositon->next2ClippedPolygon = pTempPoint;
pNextPosition->prev2ClippedPolygon = pTempPoint;
pTempPoint->prev2ClippedPolygon = pCurrentPositon;
pTempPoint->next2ClippedPolygon = pNextPosition;
break;
}
pCurrentPositon = pNextPosition;
pNextPosition = pCurrentPositon->next2ClippedPolygon;
}
continue;
}
if (FloatEqualZero(cosofToBeginPoint) && cosofToEndPoint < 0.0f)
{
//特殊点，即交点在被裁减的多边形的边上，且同在外侧
//直接抛弃，不处理
continue;
}
if (FloatEqualZero(cosofToEndPoint) && cosofToBeginPoint < 0.0f)
{
//特殊点，即交点在被裁减的多边形的边上，且同在外侧
//直接抛弃，不处理
continue;
}
//如果两点在边的同一侧，即没有交点，直接跳过
if (cosofToBeginPoint > 0.0f && cosofToEndPoint > 0.0f)
{
continue;
}
if (cosofToBeginPoint < 0.0f && cosofToEndPoint < 0.0f)
{
continue;
}
//如果有交点，则求交点并将交点插入正确的位置
//两相交线段求交点
//从而可以求出p的坐标 具体算法可以参考图形学算法
float tValue =cosofToBeginPoint;
tValue /= (cosofToBeginPoint - cosofToEndPoint);
float insertionX = pBeginPointOfClippinWindow->point.x + tValue * (pEndPointOfClippinWindow->point.x - pBeginPointOfClippinWindow->point.x);
float insertionZ = pBeginPointOfClippinWindow->point.z + tValue * (pEndPointOfClippinWindow->point.z - pBeginPointOfClippinWindow->point.z);
//检查点是否在线段的延长线上
Point point = {insertionX, insertionZ};
Point a = pBeginPointOfClippedPolygon->point;
Point b = pEndPointOfClippedPolygon->point;
MakeRectByPoint(&checkRect, &a, &b);
if(true != InRect(&checkRect, &point))
{
continue;
}
a = pBeginPointOfClippinWindow->point;
b = pEndPointOfClippinWindow->point;
MakeRectByPoint(&checkRect, &a, &b);
if(true != InRect(&checkRect, &point))
{
continue;
}
ClippingInterPoint* pTempPoint = (ClippingInterPoint*)malloc(sizeof(ClippingInterPoint));
++allPointCountForClipping;
pTempPoint->point = point;
pTempPoint->tValueForClippingWindow = tValue;
pTempPoint->tValueForClippedPolygon = GetTValue(&pBeginPointOfClippedPolygon->point, &pEndPointOfClippedPolygon->point, &point);
//通过遍历链表查找正确的位置插入生成点
ClippingInterPoint* pCurrentPositon = &pointChainForClippingWindow[indexOfClippingWindow];
ClippingInterPoint* pNextPosition = pCurrentPositon->next2ClippingdWindow;
ClippingInterPoint* pEndPosition = &pointChainForClippingWindow[nextIndexOfClippingWindow];
bool pointIsAdd = false;
while(true)
{
if (IsSamePoint(&pTempPoint->point, &pCurrentPositon->point))
{
//检查该点以否已经添加,如果已经添加则退出循环
//防止重复添加导致在后面在裁剪的时候死循环
pointIsAdd = true;
break;
}
if( pTempPoint->tValueForClippingWindow < pCurrentPositon->tValueForClippingWindow)
{
ClippingInterPoint* pLastPosition = pCurrentPositon->prev2ClippingdWindow;
pLastPosition->next2ClippingdWindow = pTempPoint;
pCurrentPositon->prev2ClippingdWindow = pTempPoint;
pTempPoint->prev2ClippingdWindow = pLastPosition;
pTempPoint->next2ClippingdWindow = pCurrentPositon;
break;
}
else if (pEndPosition == pNextPosition)
{
pCurrentPositon->next2ClippingdWindow = pTempPoint;
pNextPosition->prev2ClippingdWindow = pTempPoint;
pTempPoint->prev2ClippingdWindow = pCurrentPositon;
pTempPoint->next2ClippingdWindow = pNextPosition;
break;
}
pCurrentPositon = pNextPosition;
pNextPosition = pCurrentPositon->next2ClippingdWindow;
}
if (true == pointIsAdd)
{
free(pTempPoint);
continue;
}
//通过遍历链表查找正确的位置插入生成点
pCurrentPositon = &pointChainForClippedPolygon[indexOfclippedPolygon];
pNextPosition = pCurrentPositon->next2ClippedPolygon;
pEndPosition = &pointChainForClippedPolygon[nextIndexOfClippedPolygon];
while (true)
{
if( pTempPoint->tValueForClippedPolygon < pCurrentPositon->tValueForClippedPolygon)
{
ClippingInterPoint* pLastPosition = pCurrentPositon->prev2ClippedPolygon;
pLastPosition->next2ClippedPolygon = pTempPoint;
pCurrentPositon->prev2ClippedPolygon = pTempPoint;
pTempPoint->prev2ClippedPolygon = pLastPosition;
pTempPoint->next2ClippedPolygon = pCurrentPositon;
break;
}
else if (pEndPosition == pNextPosition)
{
pCurrentPositon->next2ClippedPolygon = pTempPoint;
pNextPosition->prev2ClippedPolygon = pTempPoint;
pTempPoint->prev2ClippedPolygon = pCurrentPositon;
pTempPoint->next2ClippedPolygon = pNextPosition;
break;
}
pCurrentPositon = pNextPosition;
pNextPosition = pCurrentPositon->next2ClippedPolygon;
}
if (cosofToBeginPoint < 0.0f && cosofToEndPoint > 0.0f)
{
pTempPoint->pointType = INTER_POINT_TYPE_INTERSECTION_POINT_FOR_IN;
}
else if(cosofToBeginPoint > 0.0f && cosofToEndPoint < 0.0f)
{
pTempPoint->pointType = INTER_POINT_TYPE_INTERSECTION_POINT_FOR_OUT;
}
else
{
assert(0);
}
}
}
//完成链表构建
//开始生成convex polygon
//首先检查是否有足够的空间容纳结果
int* bufferForPolygonConverCount = (int*)malloc(sizeof(int*) * allPointCountForClipping);
GenerateConvexPolygon(pointChainForClippedPolygon, pointCountForClippedPolygon, NULL, bufferForPolygonConverCount, allPointCountForClipping);
//完成链表构建
//开始生成convex polygon
for (int i = 0; i < pointCountForClippedPolygon; ++i)
{
pointChainForClippedPolygon[i].state = PROCESS_STATE_UNPROCESSED;
int checkResult = PointInPolygon(clippingWindow, pointChainForClippedPolygon[i].point.x, pointChainForClippedPolygon[i].point.z);
if (checkResult >= 0)
{
pointChainForClippedPolygon[i].state = PROCESS_STATE_NO_NEED_PROCESS;
}
}
GenerateConvexPolygon(pointChainForClippedPolygon, pointCountForClippedPolygon, queue, bufferForPolygonConverCount, allPointCountForClipping);
//释放分配的内存
ClippingInterPoint* pCurrent = &pointChainForClippedPolygon[0];
ClippingInterPoint* pNext = pCurrent->next2ClippedPolygon;
while (pNext != &pointChainForClippedPolygon[0])
{
if (INTER_POINT_TYPE_INTERSECTION_POINT_FOR_IN == pNext->pointType || INTER_POINT_TYPE_INTERSECTION_POINT_FOR_OUT == pNext->pointType)
{
pCurrent = pNext;
pNext = pNext->next2ClippedPolygon;
free(pCurrent);
}
else
{
pNext = pNext->next2ClippedPolygon;
}
}
Queue* checkQueue = CreateQueue(GetDataCountFromQueue(queue));
while (MeshPolygon* checkPolygon = (MeshPolygon*)PopDataFromQueue(queue))
{
if (true != IsSamePolygon(checkPolygon, clippedPolygon))
{
PushDataToQueue(checkQueue, (void*)checkPolygon);
}
}
ShiftQueueData(queue, checkQueue);
ReleaseQueue(checkQueue);
free(bufferForPolygonConverCount);
free(pointChainForClippedPolygon);
free(pointChainForClippingWindow);
return 0;
}
float CVector::DotProduct(const CVector& otherVec) const
{
return VectorDotProduct(*this, otherVec);
}
void CCursor::ScreenToSpace()
{
vec3_t nearPoint , farPoint;
vec3_t mins , maxs;
trace_t trace;
vec3_t dist;
float halfWidth , halfHeight;
float projectedX , projectedY;
int point;
halfWidth = (float) m_viewport.Width / 2.0f;
halfHeight = (float) m_viewport.Height / 2.0f;
projectedX = ( (float) m_cursorPos.scrX - halfWidth ) / halfWidth;
projectedY = ( (float) m_cursorPos.scrY - halfHeight ) / halfHeight;
nearPoint[0] = projectedX;
nearPoint[1] = -projectedY;
nearPoint[2] = 0.0f;
farPoint[0] = projectedX;
farPoint[1] = -projectedY;
farPoint[2] = 1.0f;
m_invTransformMat.Transform( m_cursorPos.nearPoint , nearPoint );
m_invTransformMat.Transform( m_cursorPos.farPoint , farPoint );
VectorCopy( nearPoint , m_cursorPos.nearPoint );
VectorCopy( farPoint , m_cursorPos.farPoint );
VectorSubtract( dist , farPoint , nearPoint );
VectorNormalize( dist );
VectorMA( farPoint , nearPoint , dist , m_touchableDist );
mins[0] = -10.0f; mins[1] = -10.0f; mins[2] = -10.0f;
maxs[0] = 10.0f; maxs[1] = 10.0f; maxs[2] = 10.0f;
trace = m_world->Trace( nearPoint , mins , maxs , farPoint , MASK_PLAYERSOLID );
if( trace.fraction == 1.0f )
{
VectorCopy( m_cursorPos.farPoint , farPoint );
m_cursorPos.touched = false;
}
else
{
VectorCopy( m_cursorPos.farPoint , trace.endpos );
m_cursorPos.touched = true;
}
point = (int) m_cursorPos.farPoint[0];
m_cursorPos.farPoint[0] = (float) point;
point = (int) m_cursorPos.farPoint[1];
m_cursorPos.farPoint[1] = (float) point;
point = (int) m_cursorPos.farPoint[2];
m_cursorPos.farPoint[2] = (float) point;
VectorSubtract( dist , m_cursorPos.farPoint , m_cursorPos.nearPoint );
m_farSquareDist = VectorDotProduct( dist , dist );
}
void helicalTurnCntrl(void)
{
union longww accum;
int16_t pitchAdjustAngleOfAttack;
int16_t rollErrorVector[3];
int16_t rtlkick;
int16_t desiredPitch;
int16_t steeringInput;
int16_t desiredTiltVector[3];
int16_t desiredRotationRateGyro[3];
uint16_t airSpeed;
union longww desiredTilt;
int16_t desiredPitchVector[2];
int16_t desiredPerpendicularPitchVector[2];
int16_t actualPitchVector[2];
int16_t pitchDot;
int16_t pitchCross;
int16_t pitchError;
int16_t pitchEarthBodyProjection[2];
int16_t angleOfAttack;
#ifdef TestGains
state_flags._.GPS_steering = 0; // turn off navigation
state_flags._.pitch_feedback = 1; // turn on stabilization
airSpeed = 981; // for testing purposes, an airspeed is needed
#else
airSpeed = air_speed_3DIMU;
if (airSpeed < TURN_CALC_MINIMUM_AIRSPEED) airSpeed = TURN_CALC_MINIMUM_AIRSPEED;
#endif
// determine the desired turn rate as the sum of navigation and fly by wire.
// this allows the pilot to override navigation if needed.
steeringInput = 0 ; // just in case no airframe type is specified or radio is off
if (udb_flags._.radio_on == 1)
{
#if ( (AIRFRAME_TYPE == AIRFRAME_STANDARD) || (AIRFRAME_TYPE == AIRFRAME_GLIDER) )
if (AILERON_INPUT_CHANNEL != CHANNEL_UNUSED) // compiler is smart about this
{
steeringInput = udb_pwIn[ AILERON_INPUT_CHANNEL ] - udb_pwTrim[ AILERON_INPUT_CHANNEL ];
steeringInput = REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED, steeringInput);
}
else if (RUDDER_INPUT_CHANNEL != CHANNEL_UNUSED)
{
steeringInput = udb_pwIn[ RUDDER_INPUT_CHANNEL ] - udb_pwTrim[ RUDDER_INPUT_CHANNEL ];
steeringInput = REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED, steeringInput);
}
else
{
steeringInput = 0;
}
#endif // AIRFRAME_STANDARD
#if (AIRFRAME_TYPE == AIRFRAME_VTAIL)
// use aileron channel if it is available, otherwise use rudder
if (AILERON_INPUT_CHANNEL != CHANNEL_UNUSED) // compiler is smart about this
{
steeringInput = udb_pwIn[AILERON_INPUT_CHANNEL] - udb_pwTrim[AILERON_INPUT_CHANNEL];
steeringInput = REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED, steeringInput);
}
else if (RUDDER_INPUT_CHANNEL != CHANNEL_UNUSED)
{
// unmix the Vtail
int16_t rudderInput = REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED, (udb_pwIn[ RUDDER_INPUT_CHANNEL] - udb_pwTrim[RUDDER_INPUT_CHANNEL]));
int16_t elevatorInput = REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED, (udb_pwIn[ ELEVATOR_INPUT_CHANNEL] - udb_pwTrim[ELEVATOR_INPUT_CHANNEL]));
steeringInput = (-rudderInput + elevatorInput);
}
else
{
steeringInput = 0;
}
#endif // AIRFRAME_VTAIL
#if (AIRFRAME_TYPE == AIRFRAME_DELTA)
// delta wing must have an aileron input, so use that
// unmix the elevons
int16_t aileronInput = REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED, (udb_pwIn[AILERON_INPUT_CHANNEL] - udb_pwTrim[AILERON_INPUT_CHANNEL]));
int16_t elevatorInput = REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED, (udb_pwIn[ELEVATOR_INPUT_CHANNEL] - udb_pwTrim[ELEVATOR_INPUT_CHANNEL]));
steeringInput = REVERSE_IF_NEEDED(ELEVON_VTAIL_SURFACES_REVERSED, ((elevatorInput - aileronInput)));
#endif // AIRFRAME_DELTA
}
if (steeringInput > MAX_INPUT) steeringInput = MAX_INPUT;
if (steeringInput < - MAX_INPUT) steeringInput = - MAX_INPUT;
// note that total steering is the sum of pilot input and waypoint navigation,
// so that the pilot always has some say in the matter
accum.WW = __builtin_mulsu(steeringInput, turngainfbw) /(2*MAX_INPUT);
if ((settings._.AileronNavigation || settings._.RudderNavigation) && state_flags._.GPS_steering)
{
accum.WW +=(int32_t) navigate_determine_deflection('t');
}
if (accum.WW >(int32_t) 2*(int32_t) RMAX - 1) accum.WW =(int32_t) 2*(int32_t) RMAX - 1;
if (accum.WW < -(int32_t) 2*(int32_t) RMAX + 1) accum.WW = -(int32_t) 2*(int32_t) RMAX + 1;
desiredTurnRateRadians = accum._.W0;
// compute the desired tilt from desired turn rate and air speed
// range for acceleration is plus minus 4 times gravity
// range for turning rate is plus minus 4 radians per second
// desiredTilt is the ratio(-rmat[6]/rmat[8]), times RMAX/2 required for the turn
// desiredTilt = desiredTurnRate * airSpeed / gravity
// desiredTilt = RMAX/2*"real desired tilt"
// desiredTurnRate = RMAX/2*"real desired turn rate", desired turn rate in radians per second
// airSpeed is air speed centimeters per second
// gravity is 981 centimeters per second per second
desiredTilt.WW = - __builtin_mulsu(desiredTurnRateRadians, airSpeed);
desiredTilt.WW /= GRAVITYCMSECSEC;
// limit the lateral acceleration to +- 4 times gravity, total wing loading approximately 4.12 times gravity
if (desiredTilt.WW > (int32_t)2 * (int32_t)RMAX - 1)
{
desiredTilt.WW = (int32_t)2 * (int32_t)RMAX - 1;
accum.WW = __builtin_mulsu(-desiredTilt._.W0, GRAVITYCMSECSEC);
accum.WW /= airSpeed;
desiredTurnRateRadians = accum._.W0;
}
else if (desiredTilt.WW < -(int32_t)2 * (int32_t)RMAX + 1)
{
desiredTilt.WW = -(int32_t)2 * (int32_t)RMAX + 1;
accum.WW = __builtin_mulsu(-desiredTilt._.W0, GRAVITYCMSECSEC);
accum.WW /= airSpeed;
desiredTurnRateRadians = accum._.W0;
}
// Compute the amount of lift needed to perform the desired turn
// Tests show that the best estimate of lift is obtained using
// actual values of rmat[6] and rmat[8], and the commanded value of their ratio
estimatedLift = wingLift(rmat[6], rmat[8], desiredTilt._.W0);
// compute angle of attack and elevator trim based on relative wing loading.
// relative wing loading is the ratio of wing loading divided by the stall wing loading, as a function of air speed
// both angle of attack and trim are computed by a linear approximation as a function of relative loading:
// y = (2m)*(x/2) + b, y is either angle of attack or elevator trim.
// x is relative wing loading. (x/2 is computed instead of x)
// 2m and b are determined from values of angle of attack and trim at stall speed, normal and inverted.
// b = (y_normal + y_inverted) / 2.
// 2m = (y_normal - y_inverted).
// If airspeed is greater than stall speed, compute angle of attack and elevator trim,
// otherwise set AoA and trim to zero.
if (air_speed_3DIMU > STALL_SPEED_CM_SEC)
{
// compute "x/2", the relative wing loading
relativeLoading = relativeWingLoading(estimatedLift, air_speed_3DIMU);
// multiply x/2 by 2m for angle of attack
accum.WW = __builtin_mulss(AOA_SLOPE, relativeLoading);
// add mx to b
angleOfAttack = AOA_OFFSET + accum._.W1;
// project angle of attack into the earth frame
accum.WW =(__builtin_mulss(angleOfAttack, rmat[8])) << 2;
pitchAdjustAngleOfAttack = accum._.W1;
// similarly, compute elevator trim
accum.WW = __builtin_mulss(ELEVATOR_TRIM_SLOPE, relativeLoading);
elevatorLoadingTrim = ELEVATOR_TRIM_OFFSET + accum._.W1;
}
else
{
angleOfAttack = 0;
pitchAdjustAngleOfAttack = 0;
elevatorLoadingTrim = 0;
}
// SetAofA(angleOfAttack); // removed by helicalTurns
// convert desired turn rate from radians/second to gyro units
accum.WW = (((int32_t)desiredTurnRateRadians) << 4); // desired turn rate in radians times 16 to provide resolution for the divide to follow
accum.WW = accum.WW / RADSTOGYRO; // at this point accum._.W0 has 2 times the required gyro signal for the turn.
// compute desired rotation rate vector in body frame, scaling is same as gyro signal
VectorScale(3, desiredRotationRateGyro, &rmat[6], accum._.W0); // this operation has side effect of dividing by 2
// compute desired rotation rate vector in body frame, scaling is in RMAX/2*radians/sec
VectorScale(3, desiredRotationRateRadians, &rmat[6], desiredTurnRateRadians); // this produces half of what we want
VectorAdd(3, desiredRotationRateRadians, desiredRotationRateRadians, desiredRotationRateRadians); // double
// incorporate roll into desired tilt vector
desiredTiltVector[0] = desiredTilt._.W0;
desiredTiltVector[1] = 0;
desiredTiltVector[2] = RMAX/2; // the divide by 2 is to account for the RMAX/2 scaling in both tilt and rotation rate
vector3_normalize(desiredTiltVector, desiredTiltVector); // make sure tilt vector has magnitude RMAX
// incorporate pitch into desired tilt vector
// compute return to launch pitch down kick for unpowered RTL
if (!udb_flags._.radio_on && state_flags._.GPS_steering)
{
rtlkick = RTLKICK;
}
else
{
rtlkick = 0;
}
// Compute Matt's glider pitch adjustment
#if (GLIDE_AIRSPEED_CONTROL == 1)
fractional aspd_pitch_adj = gliding_airspeed_pitch_adjust();
#endif
// Compute total desired pitch
#if (GLIDE_AIRSPEED_CONTROL == 1)
desiredPitch = - rtlkick + aspd_pitch_adj + pitchAltitudeAdjust;
#else
desiredPitch = - rtlkick + pitchAltitudeAdjust;
#endif
// Adjustment for inverted flight
if (!canStabilizeInverted() || !desired_behavior._.inverted)
{
// normal flight
desiredTiltVector[1] = - desiredPitch - pitchAdjustAngleOfAttack;
}
else
{
// inverted flight
desiredTiltVector[0] = - desiredTiltVector[0];
desiredTiltVector[1] = - desiredPitch - pitchAdjustAngleOfAttack - INVNPITCH; // only one of the adjustments is not zero
desiredTiltVector[2] = - desiredTiltVector[2];
}
vector3_normalize(desiredTiltVector, desiredTiltVector); // make sure tilt vector has magnitude RMAX
// compute roll error
VectorCross(rollErrorVector, &rmat[6], desiredTiltVector); // compute tilt orientation error
if (VectorDotProduct(3, &rmat[6], desiredTiltVector) < 0) // more than 90 degree error
{
vector3_normalize(rollErrorVector, rollErrorVector); // for more than 90 degrees, make the tilt error vector parallel to desired axis, with magnitude RMAX
}
tiltError[1] = rollErrorVector[1];
// compute pitch error
// start by computing the projection of earth frame pitch error to body frame
pitchEarthBodyProjection[0] = rmat[6];
pitchEarthBodyProjection[1] = rmat[8];
// normalize the projection vector and compute the cosine of the actual pitch as a side effect
actualPitchVector[1] =(int16_t) vector2_normalize(pitchEarthBodyProjection, pitchEarthBodyProjection);
// complete the actual pitch vector
actualPitchVector[0] = rmat[7];
// compute the desired pitch vector
desiredPitchVector[0] = - desiredPitch;
desiredPitchVector[1] = RMAX;
vector2_normalize(desiredPitchVector, desiredPitchVector);
// rotate desired pitch vector by 90 degrees to be able to compute cross product using VectorDot
desiredPerpendicularPitchVector[0] = desiredPitchVector[1];
desiredPerpendicularPitchVector[1] = - desiredPitchVector[0];
// compute pitchDot, the dot product of actual and desired pitch vector
// (the 2* that appears in several of the following expressions is a result of the Q2.14 format)
pitchDot = 2*VectorDotProduct(2, actualPitchVector, desiredPitchVector);
// compute pitchCross, the cross product of the actual and desired pitch vector
pitchCross = 2*VectorDotProduct(2, actualPitchVector, desiredPerpendicularPitchVector);
if (pitchDot > 0)
{
pitchError = pitchCross;
}
else
{
if (pitchCross > 0)
{
pitchError = RMAX;
}
else
{
pitchError = - RMAX;
}
}
// multiply the normalized rmat[6], rmat[8] vector by the pitch error
VectorScale(2, pitchEarthBodyProjection, pitchEarthBodyProjection, pitchError);
tiltError[0] = 2*pitchEarthBodyProjection[1];
tiltError[2] = - 2*pitchEarthBodyProjection[0];
// compute the rotation rate error vector
VectorSubtract(3, rotationRateError, omegaAccum, desiredRotationRateGyro);
}
STATIC
ISTATUS
PhysxVisibilityTesterComputePdfProcessHitCallback(
_Inout_ PVOID Context,
_In_ PCHIT Hit,
_In_ PCMATRIX ModelToWorld,
_In_ VECTOR3 ModelViewer,
_In_ POINT3 ModelHitPoint,
_In_ POINT3 WorldHitPoint
)
{
PCPHYSX_LIGHT BackLight;
PCPHYSX_GEOMETRY Geometry;
VECTOR3 ModelSurfaceNormal;
VECTOR3 NormalizedWorldSurfaceNormal;
PCPHYSX_LIGHT FrontLight;
PCPHYSX_LIGHTED_GEOMETRY LightedGeometry;
FLOAT Pdf;
PPHYSX_VISIBILITY_TESTER_COMPUTE_PDF_PROCESS_HIT_CONTEXT ProcessHitContext;
ISTATUS Status;
FLOAT SurfaceArea;
VECTOR3 WorldSurfaceNormal;
VECTOR3 WorldToLight;
ProcessHitContext = (PPHYSX_VISIBILITY_TESTER_COMPUTE_PDF_PROCESS_HIT_CONTEXT) Context;
Geometry = (PCPHYSX_GEOMETRY) Hit->Data;
PhysxGeometryGetLight(Geometry,
Hit->FrontFace,
&FrontLight);
if (FrontLight != ProcessHitContext->Light)
{
if (ProcessHitContext->FirstHit != FALSE)
{
return ISTATUS_NO_MORE_DATA;
}
return ISTATUS_SUCCESS;
}
Status = PhysxGeometryComputeNormal(Geometry,
Hit->FrontFace,
ModelHitPoint,
&ModelSurfaceNormal);
if (Status != ISTATUS_SUCCESS)
{
return Status;
}
PhysxLightedGeometryAdapterGetLightedGeometry(Geometry, &LightedGeometry);
PhysxLightedGeometryComputeSurfaceArea(LightedGeometry,
Hit->FrontFace,
&SurfaceArea);
WorldSurfaceNormal = VectorMatrixInverseTransposedMultiply(ModelToWorld,
ModelSurfaceNormal);
NormalizedWorldSurfaceNormal = VectorNormalize(WorldSurfaceNormal, NULL, NULL);
WorldToLight = PointSubtract(WorldHitPoint, ProcessHitContext->ToLight.Origin);
Pdf = SurfaceArea *
VectorDotProduct(WorldToLight, WorldToLight) /
VectorDotProduct(ProcessHitContext->ToLight.Direction, NormalizedWorldSurfaceNormal);
PhysxGeometryGetLight(Geometry,
Hit->BackFace,
&BackLight);
if (BackLight == ProcessHitContext->Light)
{
Pdf *= (FLOAT) 2.0f;
}
ProcessHitContext->Pdf += Pdf * SurfaceArea * ProcessHitContext->InverseTotalSurfaceArea;
if (ProcessHitContext->FirstHit != FALSE)
{
ProcessHitContext->ClosestWorldPointOnLight = WorldHitPoint;
ProcessHitContext->FirstHit = FALSE;
}
return ISTATUS_SUCCESS;
}
void init(void) {
float tmp,xmin,xmax,ymin,ymax,zmin,zmax,a[3],b[3];
int i,j,k,width,height;
unsigned char *data;
float plane_S[] = { 1.0, 0.0, 0.0, 0.0 };
float plane_T[] = { 0.0, 1.0, 0.0, 0.0 };
float plane_R[] = { 0.0, 0.0, 1.0, 0.0 };
float plane_Q[] = { 0.0, 0.0, 0.0, 1.0 };
glClearDepth(1.0);
glShadeModel(GL_SMOOTH);
glEnable(GL_DEPTH_TEST);
glDepthFunc(GL_LEQUAL);
glDisable(GL_CULL_FACE);
glEnable(GL_LIGHT0);
modelsize = 0;
vertexmodel = Load3DS("data/model.3ds",&num_vertexmodel);
printf("./data/model.3ds %d face\n",num_vertexmodel / 3);
xmin = xmax = ymin = ymax = zmin = zmax = 0;
for(i = 0; i < num_vertexmodel; i++) {
if(vertexmodel[(i << 3) + 0] < xmin) xmin = vertexmodel[(i << 3) + 0];
if(vertexmodel[(i << 3) + 0] > xmax) xmax = vertexmodel[(i << 3) + 0];
if(vertexmodel[(i << 3) + 1] < ymin) ymin = vertexmodel[(i << 3) + 1];
if(vertexmodel[(i << 3) + 1] > ymax) ymax = vertexmodel[(i << 3) + 1];
if(vertexmodel[(i << 3) + 2] < zmin) zmin = vertexmodel[(i << 3) + 2];
if(vertexmodel[(i << 3) + 2] > zmax) zmax = vertexmodel[(i << 3) + 2];
}
for(i = 0; i < num_vertexmodel; i++) {
vertexmodel[(i << 3) + 0] -= (xmax + xmin) / 2.0;
vertexmodel[(i << 3) + 1] -= (ymax + ymin) / 2.0;
vertexmodel[(i << 3) + 2] -= (zmax + zmin) / 2.0;
}
for(i = 0; i < num_vertexmodel; i++) {
tmp = sqrt(vertexmodel[(i << 3) + 0] * vertexmodel[(i << 3) + 0] +
vertexmodel[(i << 3) + 1] * vertexmodel[(i << 3) + 1] +
vertexmodel[(i << 3) + 2] * vertexmodel[(i << 3) + 2]);
if(tmp > modelsize) modelsize = tmp;
}
tmp = MODELSIZE / modelsize;
modelsize = MODELSIZE;
for(i = 0; i < num_vertexmodel; i++) {
vertexmodel[(i << 3) + 0] *= tmp;
vertexmodel[(i << 3) + 1] *= tmp;
vertexmodel[(i << 3) + 2] *= tmp;
}
vertexground = Load3DS("data/ground.3ds",&num_vertexground);
printf("./data/ground.3ds %d face\n",num_vertexground / 3);
planeground = (float*)malloc(sizeof(float) * 4 * num_vertexground / 3);
for(i = 0, j = 0, k = 0; i < num_vertexground; i += 3, j += 24, k += 4) {
VectorSub(&vertexground[j + 8],&vertexground[j],a);
VectorSub(&vertexground[j + 16],&vertexground[j],b);
VectorCrossProduct(a,b,&planeground[k]);
VectorNormalize(&planeground[k],&planeground[k]);
planeground[k + 3] = -VectorDotProduct(&planeground[k],&vertexground[j]);
}
glGenTextures(1,&textureground);
glBindTexture(GL_TEXTURE_2D,textureground);
if((data = LoadJPEG("data/ground.jpg",&width,&height))) {
glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_LINEAR_MIPMAP_LINEAR);
glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_LINEAR_MIPMAP_LINEAR);
glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_WRAP_S,GL_REPEAT);
glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_WRAP_T,GL_REPEAT);
gluBuild2DMipmaps(GL_TEXTURE_2D,4,width,height,GL_RGBA,GL_UNSIGNED_BYTE,data);
free(data);
}
glGenTextures(1,&texturemodel);
glBindTexture(GL_TEXTURE_2D,texturemodel);
if((data = LoadJPEG("data/model.jpg",&width,&height))) {
glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_LINEAR_MIPMAP_LINEAR);
glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_LINEAR_MIPMAP_LINEAR);
glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_WRAP_S,GL_REPEAT);
glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_WRAP_T,GL_REPEAT);
gluBuild2DMipmaps(GL_TEXTURE_2D,4,width,height,GL_RGBA,GL_UNSIGNED_BYTE,data);
free(data);
}
glGenTextures(1,&textureshadow);
glBindTexture(GL_TEXTURE_2D,textureshadow);
glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_LINEAR);
glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_WRAP_S,GL_CLAMP);
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_WRAP_T,GL_CLAMP);
glTexImage2D(GL_TEXTURE_2D,0,4,SHADOWSIZE,SHADOWSIZE,0,GL_RGBA,GL_UNSIGNED_BYTE,NULL);
glTexGenfv(GL_S,GL_EYE_PLANE,plane_S);
glTexGenfv(GL_T,GL_EYE_PLANE,plane_T);
glTexGenfv(GL_R,GL_EYE_PLANE,plane_R);
glTexGenfv(GL_Q,GL_EYE_PLANE,plane_Q);
modellist = glGenLists(1);
glNewList(modellist,GL_COMPILE);
glPushMatrix();
glBegin(GL_TRIANGLES);
for(i = 0; i < num_vertexmodel; i++) {
glNormal3fv((float*)&vertexmodel[i << 3] + 3);
glTexCoord2fv((float*)&vertexmodel[i << 3] + 6);
glVertex3fv((float*)&vertexmodel[i << 3]);
}
glEnd();
glPopMatrix();
glEndList();
groundlist = glGenLists(1);
glNewList(groundlist,GL_COMPILE);
glBegin(GL_TRIANGLES);
for(i = 0; i < num_vertexground; i++) {
glNormal3fv((float*)&vertexground[i << 3] + 3);
glTexCoord2fv((float*)&vertexground[i << 3] + 6);
glVertex3fv((float*)&vertexground[i << 3]);
}
glEnd();
glEndList();
lightlist = glGenLists(1);
glNewList(lightlist,GL_COMPILE);
glutSolidSphere(0.6,16,16);
glEndList();
}
static
ISTATUS
SphereTrace(
_In_ const void *context,
_In_ PCRAY ray,
_In_ PSHAPE_HIT_ALLOCATOR allocator,
_Out_ PHIT *hit
)
{
PCSPHERE sphere = (PCSPHERE)context;
VECTOR3 to_center = PointSubtract(sphere->center, ray->origin);
float_t distance_to_chord_midpoint = VectorDotProduct(to_center,
ray->direction);
float_t distance_to_center_squared = VectorDotProduct(to_center, to_center);
float_t distance_to_chord_midpoint_squared =
distance_to_chord_midpoint * distance_to_chord_midpoint;
float_t distance_from_chord_to_center_squared =
distance_to_center_squared - distance_to_chord_midpoint_squared;
// The ray completely misses the sphere. No intersections are possible.
if (sphere->radius_squared < distance_from_chord_to_center_squared)
{
return ISTATUS_NO_INTERSECTION;
}
float_t half_chord_length =
sqrt(sphere->radius_squared - distance_from_chord_to_center_squared);
// Ray intersects sphere and originates inside the sphere
if (distance_to_center_squared < sphere->radius_squared)
{
float_t forward_hit = distance_to_chord_midpoint + half_chord_length;
ISTATUS status = ShapeHitAllocatorAllocate(allocator,
NULL,
forward_hit,
SPHERE_BACK_FACE,
SPHERE_FRONT_FACE,
NULL,
0,
0,
hit);
if (status != ISTATUS_SUCCESS)
{
return status;
}
float_t backwards_hit = distance_to_chord_midpoint - half_chord_length;
status = ShapeHitAllocatorAllocate(allocator,
*hit,
backwards_hit,
SPHERE_BACK_FACE,
SPHERE_FRONT_FACE,
NULL,
0,
0,
hit);
return status;
}
// Ray intersects sphere and originates outside the sphere.
float_t farther_hit = distance_to_chord_midpoint + half_chord_length;
ISTATUS status = ShapeHitAllocatorAllocate(allocator,
NULL,
farther_hit,
SPHERE_BACK_FACE,
SPHERE_FRONT_FACE,
NULL,
0,
0,
hit);
if (status != ISTATUS_SUCCESS)
{
return status;
}
float_t closer_hit = distance_to_chord_midpoint - half_chord_length;
status = ShapeHitAllocatorAllocate(allocator,
*hit,
closer_hit,
SPHERE_FRONT_FACE,
SPHERE_BACK_FACE,
NULL,
0,
0,
hit);
return status;
}
itementity_t *CItemMng::SearchCursorEntity( vec3_t pos , matrix4x4_t *transMat , float minSquareDist , vec2_t mouse , vec2_t viewport )
{
itementity_t *entity;
vec3_t diff;
vec3_t scrMins , scrMaxs;
vec3_t mins , maxs;
float squareDist , tmp;
int selected = -1;
float halfWidth , halfHeight;
halfWidth = (float) viewport[0] / 2.0f;
halfHeight = (float) viewport[1] / 2.0f;
m_minSquareDist = 100000000.0f;
entity = m_linked;
while( entity )
{
if( !entity->visible )
{
entity = entity->next;
continue;
}
VectorSubtract( diff , entity->origin , pos );
squareDist = VectorDotProduct( diff , diff );
if( squareDist > m_minSquareDist )
{
entity = entity->next;
continue;
}
VectorCopy( mins , entity->mins );
VectorCopy( maxs , entity->maxs );
CalcZAxisRotate( g_camera.m_angles[ YAW ] , mins , maxs );
VectorAdd( mins , entity->origin , mins );
VectorAdd( maxs , entity->origin , maxs );
transMat->Transform( scrMins , mins );
transMat->Transform( scrMaxs , maxs );
scrMins[0] = ( 1.0f + scrMins[0] ) * halfWidth;
scrMins[1] = ( 1.0f - scrMins[1] ) * halfHeight;
scrMaxs[0] = ( 1.0f + scrMaxs[0] ) * halfWidth;
scrMaxs[1] = ( 1.0f - scrMaxs[1] ) * halfHeight;
if( scrMins[0] > scrMaxs[0] )
{
tmp = scrMaxs[0];
scrMaxs[0] = scrMins[0];
scrMins[0] = tmp;
}
if( scrMins[1] > scrMaxs[1] )
{
tmp = scrMaxs[1];
scrMaxs[1] = scrMins[1];
scrMins[1] = tmp;
}
if( mouse[0] > scrMaxs[0] || mouse[0] < scrMins[0] )
{
entity = entity->next;
continue;
}
if( mouse[1] > scrMaxs[1] || mouse[1] < scrMins[1] )
{
entity = entity->next;
continue;
}
m_minSquareDist = squareDist;
selected = entity->idx;
entity = entity->next;
}
if( selected < 0 )
return NULL;
m_MDLMng->m_focusedItem = m_entities[ selected ].idxMDLEntity;
minSquareDist = m_minSquareDist;
return &m_entities[ selected ];
}
