ChNarrowPhaseCollider::eCollSuccess CHAABBcollider::ComputeCollisions(ChMatrix33<>& R1,
Vector T1,
ChCollisionTree* oc1,
ChMatrix33<>& R2,
Vector T2,
ChCollisionTree* oc2,
eCollMode flag) {
double t1 = GetTime();
// INHERIT parent class behaviour
if (ChNarrowPhaseCollider::ComputeCollisions(R1, T1, oc1, R2, T2, oc2, flag) != ChC_RESULT_OK)
return ChC_RESULT_GENERICERROR;
// Downcasting
CHAABBTree* o1 = (CHAABBTree*)oc1;
CHAABBTree* o2 = (CHAABBTree*)oc2;
// clear the stats
this->num_bv_tests = 0;
this->num_geo_tests = 0;
// Precompute the Rabs matrix, to be used many times in AABB collisions
const double reps = (double)1e-6;
// Rabs = fabs(R)+eps
Rabs(0, 0) = myfabs(R.Get33Element(0, 0));
Rabs(0, 0) += reps;
Rabs(0, 1) = myfabs(R.Get33Element(0, 1));
Rabs(0, 1) += reps;
Rabs(0, 2) = myfabs(R.Get33Element(0, 2));
Rabs(0, 2) += reps;
Rabs(1, 0) = myfabs(R.Get33Element(1, 0));
Rabs(1, 0) += reps;
Rabs(1, 1) = myfabs(R.Get33Element(1, 1));
Rabs(1, 1) += reps;
Rabs(1, 2) = myfabs(R.Get33Element(1, 2));
Rabs(1, 2) += reps;
Rabs(2, 0) = myfabs(R.Get33Element(2, 0));
Rabs(2, 0) += reps;
Rabs(2, 1) = myfabs(R.Get33Element(2, 1));
Rabs(2, 1) += reps;
Rabs(2, 2) = myfabs(R.Get33Element(2, 2));
Rabs(2, 2) += reps;
// Now start with both top level BVs and recurse...
CollideRecurse(o1, 0, o2, 0, flag);
double t2 = GetTime();
this->query_time_secs = t2 - t1;
return ChC_RESULT_OK;
}
float mysqrt(float val)
{
start_count();
float x = val/10;
float dx;
float diff;
float min_tol = 0.00001f;
int i, flag;
flag = 0;
if ( val == 0 ) {
x = 0;
} else {
_Pragma("loopbound min 19 max 19")
for ( i=1; i<20; i++ ) {
if ( !flag ) {
dx = ( val - ( x*x ) ) / ( 2.0f * x );
x = x + dx;
diff = val - ( x*x );
if ( myfabs( diff ) <= min_tol )
flag = 1;
} else {
x =x;
}
}
}
return (x);
}
bool CHAABB::AABB_Overlap(ChMatrix33<>& B, Vector T, CHAABB *b1, CHAABB *b2)
{
ChMatrix33<> Bf;
const double reps = (double)1e-6;
// Bf = fabs(B)
Bf(0,0) = myfabs(B.Get33Element(0,0)); Bf(0,0) += reps;
Bf(0,1) = myfabs(B.Get33Element(0,1)); Bf(0,1) += reps;
Bf(0,2) = myfabs(B.Get33Element(0,2)); Bf(0,2) += reps;
Bf(1,0) = myfabs(B.Get33Element(1,0)); Bf(1,0) += reps;
Bf(1,1) = myfabs(B.Get33Element(1,1)); Bf(1,1) += reps;
Bf(1,2) = myfabs(B.Get33Element(1,2)); Bf(1,2) += reps;
Bf(2,0) = myfabs(B.Get33Element(2,0)); Bf(2,0) += reps;
Bf(2,1) = myfabs(B.Get33Element(2,1)); Bf(2,1) += reps;
Bf(2,2) = myfabs(B.Get33Element(2,2)); Bf(2,2) += reps;
return AABB_Overlap(B, Bf, T, b1, b2);
}
OSErr ComponentMover_c::CalculateAveragedWindsHdl(char *errmsg)
{
OSErr err = 0;
long i, j, numTimeSteps = (model -> GetEndTime () - model -> GetStartTime ()) / model -> GetTimeStep() + 1;
VelocityRec value, avValue;
TMover *mover;
VelocityRec wVel = {0.,0.};
Boolean bFound = false;
double pat1Theta = PI * -(0.5 + (pat1Angle / 180.0));
double pat2Theta = PI * -(0.5 + (pat2Angle / 180.0));
//WorldPoint3D refPoint3D = {0,0,0.};
VelocityRec pat1ValRef;
double pat1ValRefLength;
//refPoint3D.p = refP;
//pat1ValRef = pattern1 -> GetPatValue (refPoint3D);
pat1ValRef = pattern1 -> GetPatValue (this->refPt3D);
pat1ValRefLength = sqrt (pat1ValRef.u * pat1ValRef.u + pat1ValRef.v * pat1ValRef.v);
strcpy(errmsg,"");
// calculate handle size - number of time steps, end time - start time / time step + 1
// for each time step, start at 24 hrs (or whatever) before and average wind at each step up to current
// if no values that far back give an error
// put time and value in the handle
// if error delete
// code goes here, might want to extend handle if model run time is changed, or recreate?
// then should delete handle in case it still exists...
if (fAveragedWindsHdl)
{ // should dispose at end of run??
DisposeHandle((Handle)fAveragedWindsHdl);
fAveragedWindsHdl = 0;
}
fAveragedWindsHdl = (TimeValuePairH)_NewHandleClear(sizeof(TimeValuePair)*numTimeSteps);
if (!fAveragedWindsHdl)
{ TechError("TComponentMover::CalculateAveragedWindsHdl()", "_NewHandleClear()", 0); return -1; }
// get the wind mover, if it's constant or nonexistent this is an error, should be checked in the dialog
if (timeMoverCode == kLinkToWindMover)
{
long /* i, j,*/ m, n;
// double length, theirLengthSq, myLengthSq, dotProduct;
// VelocityRec theirVelocity,myVelocity;
//VelocityRec wVel = {0.,0.};
TMap *map;
// TMover *mover;
// Boolean bFound = false;
for (j = 0, m = model -> mapList -> GetItemCount() ; j < m && !bFound ; j++)
{
model -> mapList -> GetListItem((Ptr)&map, j);
for (i = 0, n = map -> moverList -> GetItemCount() ; i < n ; i++)
{
map -> moverList -> GetListItem((Ptr)&mover, i);
if (mover -> GetClassID() != TYPE_WINDMOVER) continue;
if (!strcmp(mover -> className, windMoverName))
{
// JLM, note: we are implicitly matching by file name above
// ((TWindMover*) mover) -> GetTimeValue (model -> modelTime, &wVel);
bFound = true;
break;
}
}
if (!bFound)
{
map = model -> uMap;
for (i = 0, n = map -> moverList -> GetItemCount() ; i < n ; i++)
{
map -> moverList -> GetListItem((Ptr)&mover, i);
if (mover -> GetClassID() != TYPE_WINDMOVER) continue;
if (!strcmp(mover -> className, windMoverName))
{
// JLM, note: we are implicitly matching by file name above
//((TWindMover*) mover) -> GetTimeValue (model -> modelTime, &wVel);
bFound = true;
break;
}
}
}
}
if (!bFound)
{
mover = (TMover*) (model->GetWindMover(false)); // see if there is a wind at all and use it
if (!mover) {
strcpy(windMoverName, ""); // clear out any value
strcpy(errmsg,"There is no wind time file associated with the component mover");
//printError("There is no wind time file associated with the component mover");
return -1;
}
strcpy(windMoverName, mover->className); // link the wind to the component mover
//print error, also check if it's a constant wind or times out of range
}
// alert code goes here if mover is not found
}
else
{
strcpy(errmsg,"There is no wind time file associated with the component mover");
// printError("There is no wind time file associated with the component mover");
return -1;
}
// check wind time file exists for whole length of run, including the past averaging hours
for (i = 0 ; i < numTimeSteps ; i++)
{
long averageTimeSteps;
double averageSpeed=0.,averageDir = 0;
Seconds timeToGetAverageFor = model -> GetStartTime () + i * model -> GetTimeStep();
Seconds startPastTime = timeToGetAverageFor - fPastHoursToAverage * 3600;
INDEXH(fAveragedWindsHdl, i).time = model -> GetStartTime () + i * model -> GetTimeStep();
//averageTimeSteps = fPastHoursToAverage+1; // for now, will change to match model time steps...
averageTimeSteps = fPastHoursToAverage; // for now, will change to match model time steps...
// code goes here, may want to change to GetStartTime, GetEndTime, then check out of range
if (i==0) err = dynamic_cast<TWindMover *>(mover)->CheckStartTime(startPastTime); //if (forTime < INDEXH(timeValues, 0).time)
if (err==-1)
{
strcpy(errmsg,"There is no wind data to average");
return err;
}
if (err==-3)
{
if (bExtrapolateWinds)
{ // GetTimeValue() already extrapolates
err = 0;
}
else
{strcpy(errmsg,"There is not enough data in your wind file for the averaging"); goto done;}
//printError("There is not enough data in your wind file for the averaging"); goto done;
}
if (err==-2)
{
//strcpy(errmsg,"No point in averaging for constant wind."); goto done;
//fPastHoursToAverage=0; err=0; // allow constant wind, only need one value though
//printError("No point in averaging for constant wind."); goto done;
averageTimeSteps = 1;
startPastTime = timeToGetAverageFor; // this doesn't really matter with constant wind
err = 0;
}
//if (forTime > INDEXH(timeValues, n - 1).time)
if (fPastHoursToAverage==0) averageTimeSteps = 1; // just use the straight wind
for (j=0;j<averageTimeSteps;j++)
{
Seconds timeToAddToAverage = startPastTime + j*3600; // eventually this will be time step...
double windSpeedToScale, windDir,theta;
// get the time file / wind mover value for this time
// get the mover first then repeat using it for the times..., but make sure get time value gets a value...
// check first value - 24, last value else will just use first/last value
// also check if it's not a time file...
// make sure in the GetMove to GetTimeValue from the averaged handle
// check here that time is in the handle...
dynamic_cast<TWindMover *>(mover)-> GetTimeValue (timeToAddToAverage, &wVel); // minus AH 07/10/2012
//windSpeedToScale = sqrt(wVel.u*wVel.u + wVel.v*wVel.v);
// code goes here, take the component first, then average ?
windSpeedToScale = wVel.u * cos (pat1Theta) + wVel.v * sin (pat1Theta);
//averageSpeed += (windSpeedToScale) * fScaleFactorAveragedWinds / pat1ValRefLength; //windSpeedToScale; //?? need the dot product too
//averageSpeed += (windSpeedToScale); //windSpeedToScale; //?? need the dot product too - seems this was done twice?
//windDir = fmod(atan2(wVel.u,wVel.v)*180/PI+360,360); // floating point remainder
windDir = atan2(wVel.u,wVel.v); //
//windDir = fmod(-180,360); // not sure what above does...
//theta = fmod(theta+180,360); // rotate the vector cause wind is FROM this direction
//r=sqrt(u*u+v*v);
// windDir = 0;
averageSpeed = averageSpeed + windSpeedToScale; // need to divide by averageTimeSteps
averageDir = averageDir + windDir;
//averageDir = averageDir + windSpeedToScale; // need to divide by averageTimeSteps
// if add up wind dir make sure it's -180 to 180 - not necessary
}
averageSpeed = averageSpeed / averageTimeSteps;
// apply power and scale - is this the right order?
if (averageSpeed<0) averageSpeed = -1. * pow(myfabs(averageSpeed),fPowerFactorAveragedWinds);
else
/*if (fPowerFactorAveragedWinds!=1.)*/ averageSpeed = pow(averageSpeed,fPowerFactorAveragedWinds);
//for now apply the scale factor in SetOptimizeVariables()
//averageSpeed = averageSpeed*fScaleFactorAveragedWinds;
// code goes here bUseMainDialogScaleFactor = true do this way leave out fSFAW, = false just use fSFAW
averageDir = averageDir / averageTimeSteps;
//avValue.u = averageSpeed*sin(averageDir/*+PI*/); // not sure about the pi
//avValue.v = averageSpeed*cos(averageDir/*+PI*/);
avValue.u = averageSpeed; // not sure about the pi
avValue.v = 0; // not sure about the pi
INDEXH(fAveragedWindsHdl, i).value = avValue;// translate back to u,v
}
done:
if (err)
{
if (fAveragedWindsHdl)
{ // should dispose at end of run??
DisposeHandle((Handle)fAveragedWindsHdl);
fAveragedWindsHdl = 0;
}
}
return err;
}
double Math::abs(double x) { return myfabs(x); }
//todo reihenfolge der tests sinnvoll anordnen
int
obb_disjoint(float B[3][3], float T[3], float a[3], float b[3])
{
register float t, s;
register int r;
float Bf[3][3];
const float reps = 1e-6;
// Bf = fabs(B)
Bf[0][0] = myfabs(B[0][0]); Bf[0][0] += reps;
Bf[0][1] = myfabs(B[0][1]); Bf[0][1] += reps;
Bf[0][2] = myfabs(B[0][2]); Bf[0][2] += reps;
Bf[1][0] = myfabs(B[1][0]); Bf[1][0] += reps;
Bf[1][1] = myfabs(B[1][1]); Bf[1][1] += reps;
Bf[1][2] = myfabs(B[1][2]); Bf[1][2] += reps;
Bf[2][0] = myfabs(B[2][0]); Bf[2][0] += reps;
Bf[2][1] = myfabs(B[2][1]); Bf[2][1] += reps;
Bf[2][2] = myfabs(B[2][2]); Bf[2][2] += reps;
#if TRACE1
printf("Box test: Bf[3][3], B[3][3], T[3], a[3], b[3]\n");
Mprintg(Bf);
Mprintg(B);
Vprintg(T);
Vprintg(a);
Vprintg(b);
#endif
// if any of these tests are one-sided, then the polyhedra are disjoint
r = 1;
// A1 x A2 = A0
t = myfabs(T[0]);
r &= (t <=
(a[0] + b[0] * Bf[0][0] + b[1] * Bf[0][1] + b[2] * Bf[0][2]));
if (!r) return 1;
// B1 x B2 = B0
s = T[0]*B[0][0] + T[1]*B[1][0] + T[2]*B[2][0];
t = myfabs(s);
r &= ( t <=
(b[0] + a[0] * Bf[0][0] + a[1] * Bf[1][0] + a[2] * Bf[2][0]));
if (!r) return 2;
// A2 x A0 = A1
t = myfabs(T[1]);
r &= ( t <=
(a[1] + b[0] * Bf[1][0] + b[1] * Bf[1][1] + b[2] * Bf[1][2]));
if (!r) return 3;
// A0 x A1 = A2
t = myfabs(T[2]);
r &= ( t <=
(a[2] + b[0] * Bf[2][0] + b[1] * Bf[2][1] + b[2] * Bf[2][2]));
if (!r) return 4;
// B2 x B0 = B1
s = T[0]*B[0][1] + T[1]*B[1][1] + T[2]*B[2][1];
t = myfabs(s);
r &= ( t <=
(b[1] + a[0] * Bf[0][1] + a[1] * Bf[1][1] + a[2] * Bf[2][1]));
if (!r) return 5;
// B0 x B1 = B2
s = T[0]*B[0][2] + T[1]*B[1][2] + T[2]*B[2][2];
t = myfabs(s);
r &= ( t <=
(b[2] + a[0] * Bf[0][2] + a[1] * Bf[1][2] + a[2] * Bf[2][2]));
if (!r) return 6;
// A0 x B0
s = T[2] * B[1][0] - T[1] * B[2][0];
t = myfabs(s);
r &= ( t <=
(a[1] * Bf[2][0] + a[2] * Bf[1][0] +
b[1] * Bf[0][2] + b[2] * Bf[0][1]));
if (!r) return 7;
// A0 x B1
s = T[2] * B[1][1] - T[1] * B[2][1];
t = myfabs(s);
r &= ( t <=
(a[1] * Bf[2][1] + a[2] * Bf[1][1] +
b[0] * Bf[0][2] + b[2] * Bf[0][0]));
if (!r) return 8;
// A0 x B2
s = T[2] * B[1][2] - T[1] * B[2][2];
t = myfabs(s);
r &= ( t <=
(a[1] * Bf[2][2] + a[2] * Bf[1][2] +
b[0] * Bf[0][1] + b[1] * Bf[0][0]));
if (!r) return 9;
// A1 x B0
s = T[0] * B[2][0] - T[2] * B[0][0];
t = myfabs(s);
r &= ( t <=
(a[0] * Bf[2][0] + a[2] * Bf[0][0] +
b[1] * Bf[1][2] + b[2] * Bf[1][1]));
if (!r) return 10;
// A1 x B1
s = T[0] * B[2][1] - T[2] * B[0][1];
t = myfabs(s);
r &= ( t <=
(a[0] * Bf[2][1] + a[2] * Bf[0][1] +
b[0] * Bf[1][2] + b[2] * Bf[1][0]));
if (!r) return 11;
// A1 x B2
s = T[0] * B[2][2] - T[2] * B[0][2];
t = myfabs(s);
r &= (t <=
(a[0] * Bf[2][2] + a[2] * Bf[0][2] +
b[0] * Bf[1][1] + b[1] * Bf[1][0]));
if (!r) return 12;
// A2 x B0
s = T[1] * B[0][0] - T[0] * B[1][0];
t = myfabs(s);
r &= (t <=
(a[0] * Bf[1][0] + a[1] * Bf[0][0] +
b[1] * Bf[2][2] + b[2] * Bf[2][1]));
if (!r) return 13;
// A2 x B1
s = T[1] * B[0][1] - T[0] * B[1][1];
t = myfabs(s);
r &= ( t <=
(a[0] * Bf[1][1] + a[1] * Bf[0][1] +
b[0] * Bf[2][2] + b[2] * Bf[2][0]));
if (!r) return 14;
// A2 x B2
s = T[1] * B[0][2] - T[0] * B[1][2];
t = myfabs(s);
r &= ( t <=
(a[0] * Bf[1][2] + a[1] * Bf[0][2] +
b[0] * Bf[2][1] + b[1] * Bf[2][0]));
if (!r) return 15;
return 0; // should equal 0
}
int main()
{
int i,j,n;
//举例 max_left[i]表示a[0,i]中连续变量的最大和
//max_right[i]表示a[i,n - 1]中连续变量的最大和
int a[N],max_left[N],min_left[N],max_right[N],min_right[N];
int max,max_until[N],min,min_until[N];
scanf("%d",&n);
for(i = 0; i < n;i++)
{
scanf("%d",&a[i]);
}
max_until[0] = a[0];
max = max_until[0];//max_until表示包含a[0,i]且包含a[i](或者a[i,n - 1]且包含a[i])的最大和
max_left[0] = max_until[0];
for(i = 1;i < n;i++)
{
if(a[i] + max_until[i - 1] > max_until[i - 1] && a[i] + max_until[i - 1] > a[i])
{
max_until[i] = a[i] + max_until[i - 1];
max = max > max_until[i] ? max : max_until[i];
max_left[i] = max;
}
else
{
max_until[i] = a[i];
max = max > max_until[i] ? max : max_until[i];
max_left[i] = max;
}
}
min_until[0] = a[0];
min = min_until[0];//min_until表示包含a[0,i]且包含a[i](或者a[i,n - 1]且包含a[i])的最小和
min_left[0] = min_until[0];
for(i = 1;i < n;i++)
{
if(a[i] + min_until[i - 1] < a[i])
{
min_until[i] = a[i] + min_until[i - 1];
min = min < min_until[i] ? min : min_until[i];
min_left[i] = min;
}
else
{
min_until[i] = a[i];
min = min < min_until[i] ? min : min_until[i];
min_left[i] = min;
}
}
max_until[n - 1] = a[n - 1];
max = max_until[n - 1];
max_right[n - 1] = max_until[n - 1];
for(i = n - 2;i >= 0;i--)
{
if(a[i] + max_until[i + 1] > max_until[i + 1] && a[i] + max_until[i + 1] > a[i])
{
max_until[i] = a[i] + max_until[i + 1];
max = max > max_until[i] ? max : max_until[i];
max_right[i] = max;
}
else
{
max_until[i] = a[i];
max = max > max_until[i] ? max : max_until[i];
max_right[i] = max;
}
}
min_until[n - 1] = a[n - 1];
min = min_until[n - 1];
min_right[n - 1] = min_until[n - 1];
for(i = n - 2;i >= 0;i--)
{
if(a[i] + min_until[i + 1] < a[i])
{
min_until[i] = a[i] + min_until[i + 1];
min = min < min_until[i] ? min : min_until[i];
min_right[i] = min;
}
else
{
min_until[i] = a[i];
min = min < min_until[i] ? min : min_until[i];
min_right[i] = min;
}
}
max = myfabs(max_left[0],min_right[1],min_left[0],max_right[1]);
for(i = 1;i < n - 1;i++)
{
max = max > myfabs(max_left[i],min_right[i + 1],min_left[i],max_right[i + 1]) ? max : myfabs(max_left[i],min_right[i + 1],min_left[i],max_right[i + 1]);
}
printf("%d\n",max);
return 0;
}
bool CHOBB::OBB_Overlap(ChMatrix33<>& B, Vector T, Vector a, Vector b)
{
register double t, s;
register int r;
static ChMatrix33<> Bf;
const double reps = (double)1e-6;
//Vector& a = b1->d;
//Vector& b = b2->d;
// Bf = fabs(B)
Bf(0,0) = myfabs(B.Get33Element(0,0)); Bf(0,0) += reps;
Bf(0,1) = myfabs(B.Get33Element(0,1)); Bf(0,1) += reps;
Bf(0,2) = myfabs(B.Get33Element(0,2)); Bf(0,2) += reps;
Bf(1,0) = myfabs(B.Get33Element(1,0)); Bf(1,0) += reps;
Bf(1,1) = myfabs(B.Get33Element(1,1)); Bf(1,1) += reps;
Bf(1,2) = myfabs(B.Get33Element(1,2)); Bf(1,2) += reps;
Bf(2,0) = myfabs(B.Get33Element(2,0)); Bf(2,0) += reps;
Bf(2,1) = myfabs(B.Get33Element(2,1)); Bf(2,1) += reps;
Bf(2,2) = myfabs(B.Get33Element(2,2)); Bf(2,2) += reps;
// if any of these tests are one-sided, then the polyhedra are disjoint
r = 1;
// A1 x A2 = A0
t = myfabs(T.x);
r &= (t <=
(a.x + b.x * Bf.Get33Element(0,0) + b.y * Bf.Get33Element(0,1) + b.z * Bf.Get33Element(0,2)));
if (!r) return false;
// B1 x B2 = B0
s = T.x*B.Get33Element(0,0) + T.y*B.Get33Element(1,0) + T.z*B.Get33Element(2,0);
t = myfabs(s);
r &= ( t <=
(b.x + a.x * Bf.Get33Element(0,0) + a.y * Bf.Get33Element(1,0) + a.z * Bf.Get33Element(2,0)));
if (!r) return false;
// A2 x A0 = A1
t = myfabs(T.y);
r &= ( t <=
(a.y + b.x * Bf.Get33Element(1,0) + b.y * Bf.Get33Element(1,1) + b.z * Bf.Get33Element(1,2)));
if (!r) return false;
// A0 x A1 = A2
t = myfabs(T.z);
r &= ( t <=
(a.z + b.x * Bf.Get33Element(2,0) + b.y * Bf.Get33Element(2,1) + b.z * Bf.Get33Element(2,2)));
if (!r) return false;
// B2 x B0 = B1
s = T.x*B.Get33Element(0,1) + T.y*B.Get33Element(1,1) + T.z*B.Get33Element(2,1);
t = myfabs(s);
r &= ( t <=
(b.y + a.x * Bf.Get33Element(0,1) + a.y * Bf.Get33Element(1,1) + a.z * Bf.Get33Element(2,1)));
if (!r) return false;
// B0 x B1 = B2
s = T.x*B.Get33Element(0,2) + T.y*B.Get33Element(1,2) + T.z*B.Get33Element(2,2);
t = myfabs(s);
r &= ( t <=
(b.z + a.x * Bf.Get33Element(0,2) + a.y * Bf.Get33Element(1,2) + a.z * Bf.Get33Element(2,2)));
if (!r) return false;
// A0 x B0
s = T.z * B.Get33Element(1,0) - T.y * B.Get33Element(2,0);
t = myfabs(s);
r &= ( t <=
(a.y * Bf.Get33Element(2,0) + a.z * Bf.Get33Element(1,0) +
b.y * Bf.Get33Element(0,2) + b.z * Bf.Get33Element(0,1)));
if (!r) return false;
// A0 x B1
s = T.z * B.Get33Element(1,1) - T.y * B.Get33Element(2,1);
t = myfabs(s);
r &= ( t <=
(a.y * Bf.Get33Element(2,1) + a.z * Bf.Get33Element(1,1) +
b.x * Bf.Get33Element(0,2) + b.z * Bf.Get33Element(0,0)));
if (!r) return false;
// A0 x B2
s = T.z * B.Get33Element(1,2) - T.y * B.Get33Element(2,2);
t = myfabs(s);
r &= ( t <=
(a.y * Bf.Get33Element(2,2) + a.z * Bf.Get33Element(1,2) +
b.x * Bf.Get33Element(0,1) + b.y * Bf.Get33Element(0,0)));
if (!r) return false;
// A1 x B0
s = T.x * B.Get33Element(2,0) - T.z * B.Get33Element(0,0);
t = myfabs(s);
r &= ( t <=
(a.x * Bf.Get33Element(2,0) + a.z * Bf.Get33Element(0,0) +
b.y * Bf.Get33Element(1,2) + b.z * Bf.Get33Element(1,1)));
if (!r) return false;
// A1 x B1
s = T.x * B.Get33Element(2,1) - T.z * B.Get33Element(0,1);
t = myfabs(s);
r &= ( t <=
(a.x * Bf.Get33Element(2,1) + a.z * Bf.Get33Element(0,1) +
b.x * Bf.Get33Element(1,2) + b.z * Bf.Get33Element(1,0)));
if (!r) return false;
// A1 x B2
s = T.x * B.Get33Element(2,2) - T.z * B.Get33Element(0,2);
t = myfabs(s);
r &= (t <=
(a.x * Bf.Get33Element(2,2) + a.z * Bf.Get33Element(0,2) +
b.x * Bf.Get33Element(1,1) + b.y * Bf.Get33Element(1,0)));
if (!r) return false;
// A2 x B0
s = T.y * B.Get33Element(0,0) - T.x * B.Get33Element(1,0);
t = myfabs(s);
r &= (t <=
(a.x * Bf.Get33Element(1,0) + a.y * Bf.Get33Element(0,0) +
b.y * Bf.Get33Element(2,2) + b.z * Bf.Get33Element(2,1)));
if (!r) return false;
// A2 x B1
s = T.y * B.Get33Element(0,1) - T.x * B.Get33Element(1,1);
t = myfabs(s);
r &= ( t <=
(a.x * Bf.Get33Element(1,1) + a.y * Bf.Get33Element(0,1) +
b.x * Bf.Get33Element(2,2) + b.z * Bf.Get33Element(2,0)));
if (!r) return false;
// A2 x B2
s = T.y * B.Get33Element(0,2) - T.x * B.Get33Element(1,2);
t = myfabs(s);
r &= ( t <=
(a.x * Bf.Get33Element(1,2) + a.y * Bf.Get33Element(0,2) +
b.x * Bf.Get33Element(2,1) + b.y * Bf.Get33Element(2,0)));
if (!r) return false;
return true; // no separation: BV collide!!
}
bool CHAABB::AABB_Overlap(ChMatrix33<>& B, ChMatrix33<>& Bf, Vector T, CHAABB *b1, CHAABB *b2)
{
register double t, s;
register int r;
Vector& a = b1->d;
Vector& b = b2->d;
// if any of these tests are one-sided, then the polyhedra are disjoint
r = 1;
// A1 x A2 = A0
t = myfabs(T.x);
r &= (t <=
(a.x + b.x * Bf.Get33Element(0,0) + b.y * Bf.Get33Element(0,1) + b.z * Bf.Get33Element(0,2)));
if (!r) return false;
// B1 x B2 = B0
s = T.x*B.Get33Element(0,0) + T.y*B.Get33Element(1,0) + T.z*B.Get33Element(2,0);
t = myfabs(s);
r &= ( t <=
(b.x + a.x * Bf.Get33Element(0,0) + a.y * Bf.Get33Element(1,0) + a.z * Bf.Get33Element(2,0)));
if (!r) return false;
// A2 x A0 = A1
t = myfabs(T.y);
r &= ( t <=
(a.y + b.x * Bf.Get33Element(1,0) + b.y * Bf.Get33Element(1,1) + b.z * Bf.Get33Element(1,2)));
if (!r) return false;
// A0 x A1 = A2
t = myfabs(T.z);
r &= ( t <=
(a.z + b.x * Bf.Get33Element(2,0) + b.y * Bf.Get33Element(2,1) + b.z * Bf.Get33Element(2,2)));
if (!r) return false;
// B2 x B0 = B1
s = T.x*B.Get33Element(0,1) + T.y*B.Get33Element(1,1) + T.z*B.Get33Element(2,1);
t = myfabs(s);
r &= ( t <=
(b.y + a.x * Bf.Get33Element(0,1) + a.y * Bf.Get33Element(1,1) + a.z * Bf.Get33Element(2,1)));
if (!r) return false;
// B0 x B1 = B2
s = T.x*B.Get33Element(0,2) + T.y*B.Get33Element(1,2) + T.z*B.Get33Element(2,2);
t = myfabs(s);
r &= ( t <=
(b.z + a.x * Bf.Get33Element(0,2) + a.y * Bf.Get33Element(1,2) + a.z * Bf.Get33Element(2,2)));
if (!r) return false;
// A0 x B0
s = T.z * B.Get33Element(1,0) - T.y * B.Get33Element(2,0);
t = myfabs(s);
r &= ( t <=
(a.y * Bf.Get33Element(2,0) + a.z * Bf.Get33Element(1,0) +
b.y * Bf.Get33Element(0,2) + b.z * Bf.Get33Element(0,1)));
if (!r) return false;
// A0 x B1
s = T.z * B.Get33Element(1,1) - T.y * B.Get33Element(2,1);
t = myfabs(s);
r &= ( t <=
(a.y * Bf.Get33Element(2,1) + a.z * Bf.Get33Element(1,1) +
b.x * Bf.Get33Element(0,2) + b.z * Bf.Get33Element(0,0)));
if (!r) return false;
// A0 x B2
s = T.z * B.Get33Element(1,2) - T.y * B.Get33Element(2,2);
t = myfabs(s);
r &= ( t <=
(a.y * Bf.Get33Element(2,2) + a.z * Bf.Get33Element(1,2) +
b.x * Bf.Get33Element(0,1) + b.y * Bf.Get33Element(0,0)));
if (!r) return false;
// A1 x B0
s = T.x * B.Get33Element(2,0) - T.z * B.Get33Element(0,0);
t = myfabs(s);
r &= ( t <=
(a.x * Bf.Get33Element(2,0) + a.z * Bf.Get33Element(0,0) +
b.y * Bf.Get33Element(1,2) + b.z * Bf.Get33Element(1,1)));
if (!r) return false;
// A1 x B1
s = T.x * B.Get33Element(2,1) - T.z * B.Get33Element(0,1);
t = myfabs(s);
r &= ( t <=
(a.x * Bf.Get33Element(2,1) + a.z * Bf.Get33Element(0,1) +
b.x * Bf.Get33Element(1,2) + b.z * Bf.Get33Element(1,0)));
if (!r) return false;
// A1 x B2
s = T.x * B.Get33Element(2,2) - T.z * B.Get33Element(0,2);
t = myfabs(s);
r &= (t <=
(a.x * Bf.Get33Element(2,2) + a.z * Bf.Get33Element(0,2) +
b.x * Bf.Get33Element(1,1) + b.y * Bf.Get33Element(1,0)));
if (!r) return false;
// A2 x B0
s = T.y * B.Get33Element(0,0) - T.x * B.Get33Element(1,0);
t = myfabs(s);
r &= (t <=
(a.x * Bf.Get33Element(1,0) + a.y * Bf.Get33Element(0,0) +
b.y * Bf.Get33Element(2,2) + b.z * Bf.Get33Element(2,1)));
if (!r) return false;
// A2 x B1
s = T.y * B.Get33Element(0,1) - T.x * B.Get33Element(1,1);
t = myfabs(s);
r &= ( t <=
(a.x * Bf.Get33Element(1,1) + a.y * Bf.Get33Element(0,1) +
b.x * Bf.Get33Element(2,2) + b.z * Bf.Get33Element(2,0)));
if (!r) return false;
// A2 x B2
s = T.y * B.Get33Element(0,2) - T.x * B.Get33Element(1,2);
t = myfabs(s);
r &= ( t <=
(a.x * Bf.Get33Element(1,2) + a.y * Bf.Get33Element(0,2) +
b.x * Bf.Get33Element(2,1) + b.y * Bf.Get33Element(2,0)));
if (!r) return false;
return true; // no separation: BV collide
}
