static int
is_edge(double *intensity, struct application *ap, const struct cell *here,
const struct cell *left, const struct cell *below, const struct cell *right, const struct cell *above)
{
int found_edge = 0;
if (!here) {
return 0;
}
if (here && here->c_ishit) {
if (detect_ids) {
if (left && below) {
if (here->c_id != left->c_id || here->c_id != below->c_id) {
found_edge = 1;
}
}
if (both_sides && right && above) {
if (here->c_id != right->c_id || here->c_id != above->c_id) {
found_edge = 1;
}
}
}
if (detect_regions) {
if (left && below) {
if (here->c_region != left->c_region ||here->c_region != below->c_region) {
found_edge = 1;
}
}
if (both_sides && right && above) {
if (here->c_region != right->c_region || here->c_region != above->c_region) {
found_edge = 1;
}
}
}
if (detect_distance) {
if (left && below) {
if (Abs(here->c_dist - left->c_dist) > max_dist || Abs(here->c_dist - below->c_dist) > max_dist) {
found_edge = 1;
}
}
if (both_sides && right && above) {
if (Abs(here->c_dist - right->c_dist) > max_dist || Abs(here->c_dist - above->c_dist) > max_dist) {
found_edge = 1;
}
}
}
if (detect_normals) {
if (left && below) {
if ((VDOT(here->c_normal, left->c_normal) < COSTOL) || (VDOT(here->c_normal, below->c_normal)< COSTOL)) {
found_edge = 1;
}
}
if (both_sides && right && above) {
if ((VDOT(here->c_normal, right->c_normal)< COSTOL) || (VDOT(here->c_normal, above->c_normal)< COSTOL)) {
found_edge = 1;
}
}
}
} else {
if (left && below) {
if (left->c_ishit || below->c_ishit) {
found_edge = 1;
}
}
if (both_sides && right && above) {
if (right->c_ishit || above->c_ishit) {
found_edge = 1;
}
}
}
if (found_edge) {
if (intensity && ap && antialias)
get_intensity(intensity, ap, here, left, below, right, above);
return 1;
}
return 0;
}
void CBuzzControllerEFootBot::SetWheelSpeedsFromVector(const CVector2& c_heading) {
/* Get the heading angle */
CRadians cHeadingAngle = c_heading.Angle().SignedNormalize();
/* Get the length of the heading vector */
Real fHeadingLength = c_heading.Length();
/* Clamp the speed so that it's not greater than MaxSpeed */
Real fBaseAngularWheelSpeed = Min<Real>(fHeadingLength, m_sWheelTurningParams.MaxSpeed);
/* Turning state switching conditions */
if(Abs(cHeadingAngle) <= m_sWheelTurningParams.NoTurnAngleThreshold) {
/* No Turn, heading angle very small */
m_sWheelTurningParams.TurningMechanism = SWheelTurningParams::NO_TURN;
}
else if(Abs(cHeadingAngle) > m_sWheelTurningParams.HardTurnOnAngleThreshold) {
/* Hard Turn, heading angle very large */
m_sWheelTurningParams.TurningMechanism = SWheelTurningParams::HARD_TURN;
}
else if(m_sWheelTurningParams.TurningMechanism == SWheelTurningParams::NO_TURN &&
Abs(cHeadingAngle) > m_sWheelTurningParams.SoftTurnOnAngleThreshold) {
/* Soft Turn, heading angle in between the two cases */
m_sWheelTurningParams.TurningMechanism = SWheelTurningParams::SOFT_TURN;
}
/* Wheel speeds based on current turning state */
Real fSpeed1, fSpeed2;
switch(m_sWheelTurningParams.TurningMechanism) {
case SWheelTurningParams::NO_TURN: {
/* Just go straight */
fSpeed1 = fBaseAngularWheelSpeed;
fSpeed2 = fBaseAngularWheelSpeed;
break;
}
case SWheelTurningParams::SOFT_TURN: {
/* Both wheels go straight, but one is faster than the other */
Real fSpeedFactor = (m_sWheelTurningParams.HardTurnOnAngleThreshold - Abs(cHeadingAngle)) / m_sWheelTurningParams.HardTurnOnAngleThreshold;
fSpeed1 = fBaseAngularWheelSpeed - fBaseAngularWheelSpeed * (1.0 - fSpeedFactor);
fSpeed2 = fBaseAngularWheelSpeed + fBaseAngularWheelSpeed * (1.0 - fSpeedFactor);
break;
}
case SWheelTurningParams::HARD_TURN: {
/* Opposite wheel speeds */
fSpeed1 = -m_sWheelTurningParams.MaxSpeed;
fSpeed2 = m_sWheelTurningParams.MaxSpeed;
break;
}
}
/* Apply the calculated speeds to the appropriate wheels */
Real fLeftWheelSpeed, fRightWheelSpeed;
if(cHeadingAngle > CRadians::ZERO) {
/* Turn Left */
fLeftWheelSpeed = fSpeed1;
fRightWheelSpeed = fSpeed2;
}
else {
/* Turn Right */
fLeftWheelSpeed = fSpeed2;
fRightWheelSpeed = fSpeed1;
}
/* Finally, set the wheel speeds */
m_pcWheels->SetLinearVelocity(fLeftWheelSpeed, fRightWheelSpeed);
}
void Text::SetEffectStrokeThickness(int thickness)
{
strokeThickness_ = Abs(thickness);
}
bool CCylinder::Intersects(Real& f_t_on_ray,
const CRay3& c_ray) {
/*
* This algorithm was adapted from
* http://www.realtimerendering.com/resources/GraphicsGems/gemsiv/ray_cyl.c
*/
/* Vector from cylinder base to ray start */
CVector3 cCylBase2RayStart(c_ray.GetStart());
cCylBase2RayStart -= m_cBasePos;
/* Ray direction and length */
CVector3 cRayDir;
c_ray.GetDirection(cRayDir);
Real fRayLen = c_ray.GetLength();
/* Vector normal to cylinder axis and ray direction */
CVector3 cNormal(cRayDir);
cNormal.CrossProduct(m_cAxis);
Real fNormalLen = cNormal.Length();
/* Are cylinder axis and ray parallel? */
if(fNormalLen > 0) {
/* No, they aren't parallel */
/* Make normal have length 1 */
cNormal /= fNormalLen;
/* Calculate shortest distance between axis and ray
* by projecting cCylBase2RayStart onto cNormal */
Real fDist = Abs(cCylBase2RayStart.DotProduct(cNormal));
/* Is fDist smaller than the cylinder radius? */
if(fDist > m_fRadius) {
/* No, it's not, so there can't be any intersection */
return false;
}
/* If we get here, it's because the ray intersects the infinite cylinder */
/* Create a buffer for the 4 potential intersection points
(two on the sides, two on the bases) */
Real fPotentialT[4];
/* First, calculate the intersection points with the sides */
/* Calculate the midpoint between the two intersection points */
CVector3 cVec(cCylBase2RayStart);
cVec.CrossProduct(m_cAxis);
Real fMidPointDist = -cVec.DotProduct(cNormal) / fNormalLen;
/* Calculate the distance between the midpoint and the potential t's */
cVec = cNormal;
cVec.CrossProduct(m_cAxis);
cVec.Normalize();
Real fDeltaToMidPoint = Abs(Sqrt(Square(m_fRadius) - Square(fDist)) / cRayDir.DotProduct(cVec));
/* Calculate the potential t's on the infinite surface */
fPotentialT[0] = (fMidPointDist - fDeltaToMidPoint) / fRayLen;
fPotentialT[1] = (fMidPointDist + fDeltaToMidPoint) / fRayLen;
/* Make sure these t's correspond to points within the cylinder bases */
CVector3 cPoint;
c_ray.GetPoint(cPoint, fPotentialT[0]);
if((cPoint - m_cBasePos).DotProduct(m_cAxis) < 0 ||
(cPoint - (m_cBasePos + m_fHeight * m_cAxis)).DotProduct(m_cAxis) > 0) {
fPotentialT[0] = -1;
}
c_ray.GetPoint(cPoint, fPotentialT[1]);
if((cPoint - m_cBasePos).DotProduct(m_cAxis) < 0 ||
(cPoint - (m_cBasePos + m_fHeight * m_cAxis)).DotProduct(m_cAxis) > 0) {
fPotentialT[1] = -1;
}
/* Check whether the ray is contained within the cylinder bases */
Real fDenominator = cRayDir.DotProduct(m_cAxis);
/* Is ray parallel to plane? */
if(Abs(fDenominator) > 1e-6) {
/* No, it's not parallel */
fDenominator *= fRayLen;
/* Bottom base */
fPotentialT[2] =
(m_cBasePos - c_ray.GetStart()).DotProduct(m_cAxis) / fDenominator;
/* Top base */
fPotentialT[3] =
(m_cBasePos + m_fHeight * m_cAxis - c_ray.GetStart()).DotProduct(m_cAxis) / fDenominator;
/* Make sure these t's are within the cylinder surface */
c_ray.GetPoint(cPoint, fPotentialT[2]);
CVector3 cDiff = cPoint - m_cBasePos;
if((cDiff - cDiff.DotProduct(m_cAxis) * m_cAxis).SquareLength() > Square(m_fRadius))
fPotentialT[2] = -1;
c_ray.GetPoint(cPoint, fPotentialT[3]);
cDiff = cPoint - m_cBasePos;
if((cDiff - cDiff.DotProduct(m_cAxis) * m_cAxis).SquareLength() > Square(m_fRadius))
fPotentialT[3] = -1;
}
else {
/* Yes, it's parallel - discard the intersections */
fPotentialT[2] = -1.0;
fPotentialT[3] = -1.0;
}
/* Go through all the potential t's and get the best */
f_t_on_ray = 2.0;
for(UInt32 i = 0; i < 4; ++i) {
if(fPotentialT[i] > 0.0f) {
f_t_on_ray = Min(f_t_on_ray, fPotentialT[i]);
}
}
/* Return true only if the intersection point is within the ray limits */
return (f_t_on_ray < 1.0f);
}
else {
/* Yes, ray and axis are parallel */
/* Projection of cCylBase2RayStart onto the axis */
Real fProj = cCylBase2RayStart.DotProduct(m_cAxis);
/* Radial vector */
CVector3 cRadial(cCylBase2RayStart);
cRadial -= fProj * m_cAxis;
Real fDist = cRadial.Length();
/* Is ray within the cylinder radius? */
if(fDist > m_fRadius) {
/* No, it's not */
return false;
}
/* If we get here, it's because the ray might intersect the cylinder bases */
Real fDenominator = cRayDir.DotProduct(m_cAxis) * fRayLen;
/* Create a buffer for the 2 potential intersection points */
Real fPotentialT[2];
/* Bottom base */
fPotentialT[0] =
(m_cBasePos-c_ray.GetStart()).DotProduct(m_cAxis) / fDenominator;
/* Top base */
fPotentialT[1] =
(m_cBasePos + m_fHeight * m_cAxis - c_ray.GetStart()).DotProduct(m_cAxis) / fDenominator;
/* Make sure these t's are within the cylinder surface */
CVector3 cPoint;
c_ray.GetPoint(cPoint, fPotentialT[0]);
CVector3 cDiff = cPoint - m_cBasePos;
if((cDiff - cDiff.DotProduct(m_cAxis) * m_cAxis).SquareLength() > Square(m_fRadius))
fPotentialT[0] = -1;
c_ray.GetPoint(cPoint, fPotentialT[1]);
cDiff = cPoint - m_cBasePos;
if((cDiff - cDiff.DotProduct(m_cAxis) * m_cAxis).SquareLength() > Square(m_fRadius))
fPotentialT[1] = -1;
/* Go through all the potential t's and get the best */
f_t_on_ray = 2.0;
for(UInt32 i = 0; i < 2; ++i) {
if(fPotentialT[i] > 0.0f) {
f_t_on_ray = Min(f_t_on_ray, fPotentialT[i]);
}
}
/* Return true only if the intersection point is within the ray limits */
return (f_t_on_ray < 1.0f);
}
}
static Bool _DeformFn(BaseDocument *doc, BaseList2D *op, HairObject *hair, HairGuides *guides, Vector *padr, LONG cnt, LONG scnt)
{
LONG i,l;
BaseContainer *bc=op->GetDataInstance();
const SReal *pCombX=NULL,*pCombY=NULL,*pCombZ=NULL;
HairLibrary hlib;
RootObjectData rData;
hair->GetRootObject(NULL,NULL,&rData);
if (!rData.pObject) return TRUE;
Real strength=bc->GetReal(HAIR_DEFORMER_STRENGTH);
VertexMapTag *pVTag=(VertexMapTag*)bc->GetLink(HAIR_DEFORMER_COMB_X,doc,Tvertexmap);
if (pVTag && pVTag->GetObject()==rData.pObject) pCombX=pVTag->GetDataAddressR();
pVTag=(VertexMapTag*)bc->GetLink(HAIR_DEFORMER_COMB_Y,doc,Tvertexmap);
if (pVTag && pVTag->GetObject()==rData.pObject) pCombY=pVTag->GetDataAddressR();
pVTag=(VertexMapTag*)bc->GetLink(HAIR_DEFORMER_COMB_Z,doc,Tvertexmap);
if (pVTag && pVTag->GetObject()==rData.pObject) pCombZ=pVTag->GetDataAddressR();
if (!(pCombX || pCombY || pCombZ)) return TRUE;
const CPolygon *vadr=rData.pPolygon;
if (!padr || !vadr) return TRUE;
for (i=0;i<cnt;i++)
{
Vector comb,dn(DC);
HairRootData hroot=guides->GetRoot(i);
LONG p=hroot.m_ID;
Real s=hroot.m_S,t=hroot.m_T;
if (hroot.m_Type==HAIR_ROOT_TYPE_POLY)
{
if (pCombX) comb.x=hlib.MixST(s,t,pCombX[vadr[p].a],pCombX[vadr[p].b],pCombX[vadr[p].c],pCombX[vadr[p].d],vadr[p].c!=vadr[p].d)-0.5;
if (pCombY) comb.y=hlib.MixST(s,t,pCombY[vadr[p].a],pCombY[vadr[p].b],pCombY[vadr[p].c],pCombY[vadr[p].d],vadr[p].c!=vadr[p].d)-0.5;
if (pCombZ) comb.z=hlib.MixST(s,t,pCombZ[vadr[p].a],pCombZ[vadr[p].b],pCombZ[vadr[p].c],pCombZ[vadr[p].d],vadr[p].c!=vadr[p].d)-0.5;
}
else if (hroot.m_Type==HAIR_ROOT_TYPE_VERTEX)
{
if (pCombX) comb.x=pCombX[p];
if (pCombY) comb.y=pCombX[p];
if (pCombZ) comb.z=pCombX[p];
}
else
continue;
dn=!(padr[i*scnt+1]-padr[i*scnt]);
Real cs=Len(comb)*strength;
if (Abs(cs)<1e-5) continue;
comb=comb/cs;
dn=!Mix(dn,comb,cs);
Vector ax=comb%dn;
Real theta=dn*comb;
Matrix tm=RotAxisToMatrix(ax,theta);
for (l=1;l<scnt;l++)
{
padr[i*scnt+l]=((padr[i*scnt+l]-padr[i*scnt])^tm)+padr[i*scnt];
}
}
return TRUE;
}
const bool conjugate = ( shift0.imag() == -shift1.imag() );
if( !bothReal && !conjugate )
LogicError("Assumed shifts were either both real or conjugates");
)
if( n == 2 )
{
const Real& eta00 = H(0,0);
const Real& eta01 = H(0,1);
const Real& eta10 = H(1,0);
const Real& eta11 = H(1,1);
// It seems arbitrary whether the scale is computed relative
// to shift0 or shift1, but we follow LAPACK's convention.
// (While the choice is irrelevant for conjugate shifts, it is not for
// real shifts)
const Real scale = OneAbs(eta00-shift1) + Abs(eta10);
if( scale == zero )
{
v[0] = v[1] = zero;
}
else
{
// Normalize the first column by the scale
Real eta10Scale = eta10 / scale;
v[0] = eta10Scale*eta01 +
(eta00-shift0.real())*((eta00-shift1.real())/scale) -
shift0.imag()*(shift1.imag()/scale);
v[1] = eta10Scale*(eta00+eta11-shift0.real()-shift1.real());
}
}
else
//
//#############################################################################
//#############################################################################
//
UnitQuaternion&
UnitQuaternion::Subtract(
const UnitVector3D &end,
const UnitVector3D &start
)
{
Check_Pointer(this);
Check_Object(&start);
Check_Object(&end);
Vector3D
axis;
SinCosPair
delta;
delta.cosine = start*end;
//
//----------------------------------------------------------------------
// See if the vectors point in the same direction. If so, return a null
// rotation
//----------------------------------------------------------------------
//
if (Close_Enough(delta.cosine, 1.0f))
{
x = 0.0f;
y = 0.0f;
z = 0.0f;
w = 1.0f;
}
//
//-------------------------------------------------------------------------
// See if the vectors directly oppose each other. If so, pick the smallest
// axis coordinate and generate a vector along it. Project this onto the
// base vector and subtract it out, leaving a perpendicular projection.
// Extend that out to unit length, then set the angle to PI
//-------------------------------------------------------------------------
//
else if (Close_Enough(delta.cosine, -1.0f))
{
//
//---------------------------
// Pick out the smallest axis
//---------------------------
//
int
smallest=0;
Scalar
value=2.0f;
for (int i=X_Axis; i<=Z_Axis; ++i)
{
if (Abs(start[i]) < value)
{
smallest = i;
value = Abs(start[i]);
}
}
//
//----------------------------------------
// Set up a vector along the selected axis
//----------------------------------------
//
axis.x = 0.0f;
axis.y = 0.0f;
axis.z = 0.0f;
axis[smallest] = 1.0f;
//
//-------------------------------------------------------------------
// If the value on that axis wasn't zero, subtract out the projection
//-------------------------------------------------------------------
//
if (!Small_Enough(value))
{
Vector3D t;
t.Multiply(start, start*axis);
axis.Subtract(axis, t);
axis.Normalize(axis);
}
//
//----------------------
// Convert to quaternion
//----------------------
//
x = axis.x;
y = axis.y;
z = axis.z;
w = 0.0f;
}
//
//--------------------------------------------------
// Otherwise, generate the cross product and unitize
//--------------------------------------------------
//
else
{
axis.Cross(start, end);
delta.sine = axis.GetLength();
axis /= delta.sine;
//
//---------------------------------------------------------------
// Now compute sine and cosine of half the angle and generate the
// quaternion
//---------------------------------------------------------------
//
delta.sine = Sqrt((1.0f - delta.cosine)*0.5f);
x = axis.x * delta.sine;
y = axis.y * delta.sine;
z = axis.z * delta.sine;
w = Sqrt((1.0f + delta.cosine)*0.5f);
}
return *this;
}
void AABB::SetFrom(const OBB &obb)
{
vec halfSize = Abs(obb.axis[0]*obb.r[0]) + Abs(obb.axis[1]*obb.r[1]) + Abs(obb.axis[2]*obb.r[2]);
SetFromCenterAndSize(obb.pos, 2.f*halfSize);
}
bool EqualRel(float a, float b, float maxRelError)
{
if (a == b) return true; // Handles the special case where a and b are both zero.
float relativeError = Abs((a-b)/Max(a, b));
return relativeError <= maxRelError;
}
bool Cylinder::Intersects( const Segment& s, CollisionInfo* const pInfo /*= NULL*/ ) const
{
Vector d = m_Point2 - m_Point1; // Cylinder axis
Vector m = s.m_Point1 - m_Point1; // Vector from cylinder base to segment base?
Vector n = s.m_Point2 - s.m_Point1; // Segment vector
float md = m.Dot( d );
float nd = n.Dot( d );
float dd = d.Dot( d );
if( md < 0.0f && md + nd < 0.0f )
{
return false;
}
if( md > dd && md + nd > dd )
{
return false;
}
float nn = n.Dot( n );
float mn = m.Dot( n );
float a = dd * nn - nd * nd;
float k = m.Dot( m ) - m_Radius * m_Radius;
float c = dd * k - md * md;
float t = 0.0f;
if( Abs( a ) < SMALLER_EPSILON )
{
// Segment (n) runs parallel to cylinder axis (d)
if( c > 0.0f )
{
return false;
}
if( md < 0.0f )
{
t = -mn / nn;
}
else if( md > dd )
{
t = ( nd - mn ) / nn;
}
else
{
// TODO: This seems to be problematic (or getting here is indicative of an earlier problem)
WARNDESC( "THAT CYLINDER COLLISION BUG" );
t = 0.0f;
}
if( pInfo )
{
Vector Intersection = s.m_Point1 + t * n;
Vector PointOnLine = Line( m_Point1, m_Point2 - m_Point1 ).NearestPointTo( pInfo->m_Intersection );
Vector Normal = ( Intersection - PointOnLine ).GetNormalized();
pInfo->m_Collision = true;
pInfo->m_Intersection = Intersection;
pInfo->m_HitT = t;
pInfo->m_Plane = Plane( Normal, PointOnLine );
}
return true;
}
float b = dd * mn - nd * md;
float Discr = b * b - a * c;
if( Discr < 0.0f )
{
return false;
}
t = ( -b - SqRt( Discr ) ) / a;
if( t < 0.0f || t > 1.0f )
{
return false;
}
// Test endcaps--if we collide with them, count it as not colliding with cylinder
if( md + t * nd < 0.0f )
{
return false;
//// Segment outside cylinder on first side
//if( nd <= 0.0f )
//{
// // Segment pointing away from endcap
// return false;
//}
//float t2 = -md / nd;
//if( k + t2 * ( 2.0f * mn + t2 * nn ) > 0.0f )
//{
// return false;
//}
}
else if( md + t * nd > dd )
{
return false;
//// Segment outside cylinder on second side
//if( nd >= 0.0f )
//{
// // Segment pointing away from endcap
// return false;
//}
//float t2 = ( dd - md ) / nd;
//if( k + dd - 2.0f * md + t2 * ( 2.0f * ( mn - nd ) + t2 * nn ) > 0.0f )
//{
// return false;
//}
}
if( pInfo )
{
Vector Intersection = s.m_Point1 + t * n;
Vector PointOnLine = Line( m_Point1, m_Point2 - m_Point1 ).NearestPointTo( Intersection );
Vector Normal = ( Intersection - PointOnLine ).GetNormalized();
pInfo->m_Collision = true;
pInfo->m_Intersection = Intersection;
pInfo->m_HitT = t;
pInfo->m_Plane = Plane( Normal, PointOnLine );
}
return true;
}
static float
calc_rank_and(float *w, tsvector * t, QUERYTYPE * q)
{
uint16 **pos;
int i,
k,
l,
p;
WordEntry *entry;
WordEntryPos *post,
*ct;
int4 dimt,
lenct,
dist;
float res = -1.0;
ITEM **item;
int size = q->size;
item = SortAndUniqItems(GETOPERAND(q), GETQUERY(q), &size);
if (size < 2)
{
pfree(item);
return calc_rank_or(w, t, q);
}
pos = (uint16 **) palloc(sizeof(uint16 *) * q->size);
memset(pos, 0, sizeof(uint16 *) * q->size);
*(uint16 *) POSNULL = lengthof(POSNULL) - 1;
WEP_SETPOS(POSNULL[1], MAXENTRYPOS - 1);
for (i = 0; i < size; i++)
{
entry = find_wordentry(t, q, item[i]);
if (!entry)
continue;
if (entry->haspos)
pos[i] = (uint16 *) _POSDATAPTR(t, entry);
else
pos[i] = (uint16 *) POSNULL;
dimt = *(uint16 *) (pos[i]);
post = (WordEntryPos *) (pos[i] + 1);
for (k = 0; k < i; k++)
{
if (!pos[k])
continue;
lenct = *(uint16 *) (pos[k]);
ct = (WordEntryPos *) (pos[k] + 1);
for (l = 0; l < dimt; l++)
{
for (p = 0; p < lenct; p++)
{
dist = Abs((int) WEP_GETPOS(post[l]) - (int) WEP_GETPOS(ct[p]));
if (dist || (dist == 0 && (pos[i] == (uint16 *) POSNULL || pos[k] == (uint16 *) POSNULL)))
{
float curw;
if (!dist)
dist = MAXENTRYPOS;
curw = sqrt(wpos(post[l]) * wpos(ct[p]) * word_distance(dist));
res = (res < 0) ? curw : 1.0 - (1.0 - res) * (1.0 - curw);
}
}
}
}
}
pfree(pos);
pfree(item);
return res;
}
/** The following code is from Christer Ericson's book Real-Time Collision Detection, pp. 101-106.
http://realtimecollisiondetection.net/ */
bool OBB::Intersects(const OBB &b, float epsilon) const
{
assume(pos.IsFinite());
assume(b.pos.IsFinite());
assume(float3::AreOrthonormal(axis[0], axis[1], axis[2]));
assume(float3::AreOrthonormal(b.axis[0], b.axis[1], b.axis[2]));
// Generate a rotation matrix that transforms from world space to this OBB's coordinate space.
float3x3 R;
for(int i = 0; i < 3; ++i)
for(int j = 0; j < 3; ++j)
R[i][j] = Dot(axis[i], b.axis[j]);
float3 t = b.pos - pos;
// Express the translation vector in a's coordinate frame.
t = float3(Dot(t, axis[0]), Dot(t, axis[1]), Dot(t, axis[2]));
float3x3 AbsR;
for(int i = 0; i < 3; ++i)
for(int j = 0; j < 3; ++j)
AbsR[i][j] = Abs(R[i][j]) + epsilon;
// Test the three major axes of this OBB.
for(int i = 0; i < 3; ++i)
{
float ra = r[i];
float rb = DOT3(b.r, AbsR[i]);
if (Abs(t[i]) > ra + rb)
return false;
}
// Test the three major axes of the OBB b.
for(int i = 0; i < 3; ++i)
{
float ra = r[0] * AbsR[0][i] + r[1] * AbsR[1][i] + r[2] * AbsR[2][i];
float rb = b.r[i];
if (Abs(t.x + R[0][i] + t.y * R[1][i] + t.z * R[2][i]) > ra + rb)
return false;
}
// Test the 9 different cross-axes.
// A.x <cross> B.x
float ra = r.y * AbsR[2][0] + r.z * AbsR[1][0];
float rb = b.r.y * AbsR[0][2] + b.r.z * AbsR[0][1];
if (Abs(t.z * R[1][0] - t.y * R[2][0]) > ra + rb)
return false;
// A.x < cross> B.y
ra = r.y * AbsR[2][1] + r.z * AbsR[1][1];
rb = b.r.x * AbsR[0][2] + b.r.z * AbsR[0][0];
if (Abs(t.z * R[1][1] - t.y * R[2][1]) > ra + rb)
return false;
// A.x <cross> B.z
ra = r.y * AbsR[2][2] + r.z * AbsR[1][2];
rb = b.r.x * AbsR[0][1] + b.r.y * AbsR[0][0];
if (Abs(t.z * R[1][22] - t.y * R[2][2]) > ra + rb)
return false;
// A.y <cross> B.x
ra = r.x * AbsR[2][0] + r.z * AbsR[0][0];
rb = b.r.y * AbsR[1][2] + b.r.z * AbsR[1][1];
if (Abs(t.x * R[2][0] - t.z * R[0][0]) > ra + rb)
return false;
// A.y <cross> B.y
ra = r.x * AbsR[2][1] + r.z * AbsR[0][1];
rb = b.r.x * AbsR[1][2] + b.r.z * AbsR[1][0];
if (Abs(t.x * R[2][1] - t.z * R[0][1]) > ra + rb)
return false;
// A.y <cross> B.z
ra = r.x * AbsR[2][2] + r.z * AbsR[0][2];
rb = b.r.x * AbsR[1][1] + b.r.y * AbsR[1][0];
if (Abs(t.x * R[2][2] - t.z * R[0][2]) > ra + rb)
return false;
// A.z <cross> B.x
ra = r.x * AbsR[1][0] + r.y * AbsR[0][0];
rb = b.r.y * AbsR[2][2] + b.r.z * AbsR[2][1];
if (Abs(t.y * R[0][0] - t.x * R[1][0]) > ra + rb)
return false;
// A.z <cross> B.y
ra = r.x * AbsR[1][1] + r.y * AbsR[0][1];
rb = b.r.x * AbsR[2][2] + b.r.z * AbsR[2][0];
if (Abs(t.y * R[0][1] - t.x * R[1][1]) > ra + rb)
return false;
// A.z <cross> B.z
ra = r.x * AbsR[1][2] + r.y * AbsR[0][2];
rb = b.r.x * AbsR[2][1] + b.r.y * AbsR[2][0];
if (Abs(t.y * R[0][2] - t.x * R[1][2]) > ra + rb)
return false;
// No separating axis exists, so the two OBB don't intersect.
return true;
}
void CheckSticksHaveChanged(void)
{
#ifndef TESTING
static uint32 Now;
static boolean Change;
static uint8 c;
if ( F.FailsafesEnabled )
{
Now = mSClock();
if ( F.ReturnHome || F.Navigate )
{
Change = true;
mS[RxFailsafeTimeout] = Now + RC_NO_CHANGE_TIMEOUT_MS;
F.ForceFailsafe = false;
}
else
{
if ( Now > mS[StickChangeUpdate] )
{
mS[StickChangeUpdate] = Now + 500;
Change = false;
for ( c = ThrottleC; c <= (uint8)RTHRC; c++ )
{
Change |= Abs( RC[c] - RCp[c]) > RC_STICK_MOVEMENT;
RCp[c] = RC[c];
}
}
if ( Change )
{
mS[RxFailsafeTimeout] = Now + RC_NO_CHANGE_TIMEOUT_MS;
mS[NavStateTimeout] = Now;
F.ForceFailsafe = false;
if ( FailState == MonitoringRx )
{
if ( F.LostModel )
{
Beeper_OFF;
F.LostModel = false;
DescentComp = 1;
}
}
}
else
if ( Now > mS[RxFailsafeTimeout] )
{
if ( !F.ForceFailsafe && ( State == InFlight ) )
{
//Stats[RCFailsafesS]++;
mS[NavStateTimeout] = Now + NAV_RTH_LAND_TIMEOUT_MS;
mS[DescentUpdate] = Now + ALT_DESCENT_UPDATE_MS;
DescentComp = 1; // for no Baro case
F.ForceFailsafe = true;
}
}
}
}
else
F.ForceFailsafe = false;
#else
F.ForceFailsafe = false;
#endif // ENABLE_STICK_CHANGE_FAILSAFE
} // CheckSticksHaveChanged
void LUMod
( Matrix<F>& A,
Permutation& P,
const Matrix<F>& u,
const Matrix<F>& v,
bool conjugate,
Base<F> tau )
{
DEBUG_CSE
typedef Base<F> Real;
const Int m = A.Height();
const Int n = A.Width();
const Int minDim = Min(m,n);
if( minDim != m )
LogicError("It is assumed that height(A) <= width(A)");
if( u.Height() != m || u.Width() != 1 )
LogicError("u is expected to be a conforming column vector");
if( v.Height() != n || v.Width() != 1 )
LogicError("v is expected to be a conforming column vector");
// w := inv(L) P u
auto w( u );
P.PermuteRows( w );
Trsv( LOWER, NORMAL, UNIT, A, w );
// Maintain an external vector for the temporary subdiagonal of U
Matrix<F> uSub;
Zeros( uSub, minDim-1, 1 );
// Reduce w to a multiple of e0
for( Int i=minDim-2; i>=0; --i )
{
// Decide if we should pivot the i'th and i+1'th rows of w
const F lambdaSub = A(i+1,i);
const F ups_ii = A(i,i);
const F omega_i = w(i);
const F omega_ip1 = w(i+1);
const Real rightTerm = Abs(lambdaSub*omega_i+omega_ip1);
const bool pivot = ( Abs(omega_i) < tau*rightTerm );
const Range<Int> indi( i, i+1 ),
indip1( i+1, i+2 ),
indB( i+2, m ),
indR( i+1, n );
auto lBi = A( indB, indi );
auto lBip1 = A( indB, indip1 );
auto uiR = A( indi, indR );
auto uip1R = A( indip1, indR );
if( pivot )
{
// P := P_i P
P.Swap( i, i+1 );
// Simultaneously perform
// U := P_i U and
// L := P_i L P_i^T
//
// Then update
// L := L T_{i,L}^{-1},
// U := T_{i,L} U,
// w := T_{i,L} P_i w,
// where T_{i,L} is the Gauss transform which zeros (P_i w)_{i+1}.
//
// More succinctly,
// gamma := w(i) / w(i+1),
// w(i) := w(i+1),
// w(i+1) := 0,
// L(:,i) += gamma L(:,i+1),
// U(i+1,:) -= gamma U(i,:).
const F gamma = omega_i / omega_ip1;
const F lambda_ii = F(1) + gamma*lambdaSub;
A(i, i) = gamma;
A(i+1,i) = 0;
auto lBiCopy = lBi;
Swap( NORMAL, lBi, lBip1 );
Axpy( gamma, lBiCopy, lBi );
auto uip1RCopy = uip1R;
RowSwap( A, i, i+1 );
Axpy( -gamma, uip1RCopy, uip1R );
// Force L back to *unit* lower-triangular form via the transform
// L := L T_{i,U}^{-1} D^{-1},
// where D is diagonal and responsible for forcing L(i,i) and
// L(i+1,i+1) back to 1. The effect on L is:
// eta := L(i,i+1)/L(i,i),
// L(:,i+1) -= eta L(:,i),
// delta_i := L(i,i),
// delta_ip1 := L(i+1,i+1),
// L(:,i) /= delta_i,
// L(:,i+1) /= delta_ip1,
// while the effect on U is
// U(i,:) += eta U(i+1,:)
// U(i,:) *= delta_i,
// U(i+1,:) *= delta_{i+1},
// and the effect on w is
// w(i) *= delta_i.
const F eta = lambdaSub/lambda_ii;
const F delta_i = lambda_ii;
const F delta_ip1 = F(1) - eta*gamma;
Axpy( -eta, lBi, lBip1 );
A(i+1,i) = gamma/delta_i;
lBi *= F(1)/delta_i;
lBip1 *= F(1)/delta_ip1;
A(i,i) = eta*ups_ii*delta_i;
Axpy( eta, uip1R, uiR );
uiR *= delta_i;
uip1R *= delta_ip1;
uSub(i) = ups_ii*delta_ip1;
// Finally set w(i)
w(i) = omega_ip1*delta_i;
}
else
{
// Update
// L := L T_{i,L}^{-1},
// U := T_{i,L} U,
// w := T_{i,L} w,
// where T_{i,L} is the Gauss transform which zeros w_{i+1}.
//
// More succinctly,
// gamma := w(i+1) / w(i),
// L(:,i) += gamma L(:,i+1),
// U(i+1,:) -= gamma U(i,:),
// w(i+1) := 0.
const F gamma = omega_ip1 / omega_i;
A(i+1,i) += gamma;
Axpy( gamma, lBip1, lBi );
Axpy( -gamma, uiR, uip1R );
uSub(i) = -gamma*ups_ii;
}
}
// Add the modified w v' into U
{
auto a0 = A( IR(0), ALL );
const F omega_0 = w(0);
Matrix<F> vTrans;
Transpose( v, vTrans, conjugate );
Axpy( omega_0, vTrans, a0 );
}
// Transform U from upper-Hessenberg to upper-triangular form
for( Int i=0; i<minDim-1; ++i )
{
// Decide if we should pivot the i'th and i+1'th rows U
const F lambdaSub = A(i+1,i);
const F ups_ii = A(i,i);
const F ups_ip1i = uSub(i);
const Real rightTerm = Abs(lambdaSub*ups_ii+ups_ip1i);
const bool pivot = ( Abs(ups_ii) < tau*rightTerm );
const Range<Int> indi( i, i+1 ),
indip1( i+1, i+2 ),
indB( i+2, m ),
indR( i+1, n );
auto lBi = A( indB, indi );
auto lBip1 = A( indB, indip1 );
auto uiR = A( indi, indR );
auto uip1R = A( indip1, indR );
if( pivot )
{
// P := P_i P
P.Swap( i, i+1 );
// Simultaneously perform
// U := P_i U and
// L := P_i L P_i^T
//
// Then update
// L := L T_{i,L}^{-1},
// U := T_{i,L} U,
// where T_{i,L} is the Gauss transform which zeros U(i+1,i).
//
// More succinctly,
// gamma := U(i+1,i) / U(i,i),
// L(:,i) += gamma L(:,i+1),
// U(i+1,:) -= gamma U(i,:).
const F gamma = ups_ii / ups_ip1i;
const F lambda_ii = F(1) + gamma*lambdaSub;
A(i+1,i) = ups_ip1i;
A(i, i) = gamma;
auto lBiCopy = lBi;
Swap( NORMAL, lBi, lBip1 );
Axpy( gamma, lBiCopy, lBi );
auto uip1RCopy = uip1R;
RowSwap( A, i, i+1 );
Axpy( -gamma, uip1RCopy, uip1R );
// Force L back to *unit* lower-triangular form via the transform
// L := L T_{i,U}^{-1} D^{-1},
// where D is diagonal and responsible for forcing L(i,i) and
// L(i+1,i+1) back to 1. The effect on L is:
// eta := L(i,i+1)/L(i,i),
// L(:,i+1) -= eta L(:,i),
// delta_i := L(i,i),
// delta_ip1 := L(i+1,i+1),
// L(:,i) /= delta_i,
// L(:,i+1) /= delta_ip1,
// while the effect on U is
// U(i,:) += eta U(i+1,:)
// U(i,:) *= delta_i,
// U(i+1,:) *= delta_{i+1}.
const F eta = lambdaSub/lambda_ii;
const F delta_i = lambda_ii;
const F delta_ip1 = F(1) - eta*gamma;
Axpy( -eta, lBi, lBip1 );
A(i+1,i) = gamma/delta_i;
lBi *= F(1)/delta_i;
lBip1 *= F(1)/delta_ip1;
A(i,i) = ups_ip1i*delta_i;
Axpy( eta, uip1R, uiR );
uiR *= delta_i;
uip1R *= delta_ip1;
}
else
{
// Update
// L := L T_{i,L}^{-1},
// U := T_{i,L} U,
// where T_{i,L} is the Gauss transform which zeros U(i+1,i).
//
// More succinctly,
// gamma := U(i+1,i)/ U(i,i),
// L(:,i) += gamma L(:,i+1),
// U(i+1,:) -= gamma U(i,:).
const F gamma = ups_ip1i / ups_ii;
A(i+1,i) += gamma;
Axpy( gamma, lBip1, lBi );
Axpy( -gamma, uiR, uip1R );
}
}
}
inline typename Base<F>::type
HermitianFrobeniusNorm
( UpperOrLower uplo, const DistMatrix<F>& A )
{
#ifndef RELEASE
PushCallStack("internal::HermitianFrobeniusNorm");
#endif
typedef typename Base<F>::type R;
if( A.Height() != A.Width() )
throw std::logic_error("Hermitian matrices must be square.");
const int r = A.Grid().Height();
const int c = A.Grid().Width();
const int colShift = A.ColShift();
const int rowShift = A.RowShift();
R localScale = 0;
R localScaledSquare = 1;
const int localWidth = A.LocalWidth();
if( uplo == UPPER )
{
for( int jLocal=0; jLocal<localWidth; ++jLocal )
{
int j = rowShift + jLocal*c;
int numUpperRows = LocalLength(j+1,colShift,r);
for( int iLocal=0; iLocal<numUpperRows; ++iLocal )
{
int i = colShift + iLocal*r;
const R alphaAbs = Abs(A.GetLocal(iLocal,jLocal));
if( alphaAbs != 0 )
{
if( alphaAbs <= localScale )
{
const R relScale = alphaAbs/localScale;
if( i != j )
localScaledSquare += 2*relScale*relScale;
else
localScaledSquare += relScale*relScale;
}
else
{
const R relScale = localScale/alphaAbs;
if( i != j )
localScaledSquare =
localScaledSquare*relScale*relScale + 2;
else
localScaledSquare =
localScaledSquare*relScale*relScale + 1;
localScale = alphaAbs;
}
}
}
}
}
else
{
for( int jLocal=0; jLocal<localWidth; ++jLocal )
{
int j = rowShift + jLocal*c;
int numStrictlyUpperRows = LocalLength(j,colShift,r);
for( int iLocal=numStrictlyUpperRows;
iLocal<A.LocalHeight(); ++iLocal )
{
int i = colShift + iLocal*r;
const R alphaAbs = Abs(A.GetLocal(iLocal,jLocal));
if( alphaAbs != 0 )
{
if( alphaAbs <= localScale )
{
const R relScale = alphaAbs/localScale;
if( i != j )
localScaledSquare += 2*relScale*relScale;
else
localScaledSquare += relScale*relScale;
}
else
{
const R relScale = localScale/alphaAbs;
if( i != j )
localScaledSquare =
localScaledSquare*relScale*relScale + 2;
else
localScaledSquare =
localScaledSquare*relScale*relScale + 1;
localScale = alphaAbs;
}
}
}
}
}
// Find the maximum relative scale
R scale;
mpi::AllReduce( &localScale, &scale, 1, mpi::MAX, A.Grid().VCComm() );
R norm = 0;
if( scale != 0 )
{
// Equilibrate our local scaled sum to the maximum scale
R relScale = localScale/scale;
localScaledSquare *= relScale*relScale;
// The scaled square is now simply the sum of the local contributions
R scaledSquare;
mpi::AllReduce
( &localScaledSquare, &scaledSquare, 1, mpi::SUM, A.Grid().VCComm() );
norm = scale*Sqrt(scaledSquare);
}
#ifndef RELEASE
PopCallStack();
#endif
return norm;
}
static TDStrResult GetFinalDocumentImportances(
const TVector<TVector<double>>& rawImportances,
EDocumentStrengthType docImpMethod,
int topSize,
EImportanceValuesSign importanceValuesSign
) {
const ui32 trainDocCount = rawImportances.size();
Y_ASSERT(rawImportances.size() != 0);
const ui32 testDocCount = rawImportances[0].size();
TVector<TVector<double>> preprocessedImportances;
if (docImpMethod == EDocumentStrengthType::Average) {
preprocessedImportances = TVector<TVector<double>>(1, TVector<double>(trainDocCount));
for (ui32 trainDocId = 0; trainDocId < trainDocCount; ++trainDocId) {
for (ui32 testDocId = 0; testDocId < testDocCount; ++testDocId) {
preprocessedImportances[0][trainDocId] += rawImportances[trainDocId][testDocId];
}
}
for (ui32 trainDocId = 0; trainDocId < trainDocCount; ++trainDocId) {
preprocessedImportances[0][trainDocId] /= testDocCount;
}
} else {
Y_ASSERT(docImpMethod == EDocumentStrengthType::PerObject || docImpMethod == EDocumentStrengthType::Raw);
preprocessedImportances = TVector<TVector<double>>(testDocCount, TVector<double>(trainDocCount));
for (ui32 trainDocId = 0; trainDocId < trainDocCount; ++trainDocId) {
for (ui32 testDocId = 0; testDocId < testDocCount; ++testDocId) {
preprocessedImportances[testDocId][trainDocId] = rawImportances[trainDocId][testDocId];
}
}
}
TDStrResult result(preprocessedImportances.size());
for (ui32 testDocId = 0; testDocId < preprocessedImportances.size(); ++testDocId) {
TVector<double>& preprocessedImportancesRef = preprocessedImportances[testDocId];
const ui32 docCount = preprocessedImportancesRef.size();
TVector<ui32> indices(docCount);
std::iota(indices.begin(), indices.end(), 0);
if (docImpMethod != EDocumentStrengthType::Raw) {
Sort(indices.begin(), indices.end(), [&](ui32 first, ui32 second) {
return Abs(preprocessedImportancesRef[first]) > Abs(preprocessedImportancesRef[second]);
});
}
std::function<bool(double)> predicate;
if (importanceValuesSign == EImportanceValuesSign::Positive) {
predicate = [](double v){return v > 0;};
} else if (importanceValuesSign == EImportanceValuesSign::Negative) {
predicate = [](double v){return v < 0;};
} else {
Y_ASSERT(importanceValuesSign == EImportanceValuesSign::All);
predicate = [](double){return true;};
}
int currentSize = 0;
for (ui32 i = 0; i < docCount; ++i) {
if (currentSize == topSize) {
break;
}
if (predicate(preprocessedImportancesRef[indices[i]])) {
result.Scores[testDocId].push_back(preprocessedImportancesRef[indices[i]]);
result.Indices[testDocId].push_back(indices[i]);
}
++currentSize;
}
}
return result;
}
inline typename Base<F>::type
HermitianFrobeniusNorm( UpperOrLower uplo, const Matrix<F>& A )
{
#ifndef RELEASE
PushCallStack("internal::HermitianFrobeniusNorm");
#endif
typedef typename Base<F>::type R;
if( A.Height() != A.Width() )
throw std::logic_error("Hermitian matrices must be square.");
R scale = 0;
R scaledSquare = 1;
const int height = A.Height();
const int width = A.Width();
if( uplo == UPPER )
{
for( int j=0; j<width; ++j )
{
for( int i=0; i<j; ++i )
{
const R alphaAbs = Abs(A.Get(i,j));
if( alphaAbs != 0 )
{
if( alphaAbs <= scale )
{
const R relScale = alphaAbs/scale;
scaledSquare += 2*relScale*relScale;
}
else
{
const R relScale = scale/alphaAbs;
scaledSquare = scaledSquare*relScale*relScale + 2;
scale = alphaAbs;
}
}
}
const R alphaAbs = Abs(A.Get(j,j));
if( alphaAbs != 0 )
{
if( alphaAbs <= scale )
{
const R relScale = alphaAbs/scale;
scaledSquare += relScale*relScale;
}
else
{
const R relScale = scale/alphaAbs;
scaledSquare = scaledSquare*relScale*relScale + 1;
scale = alphaAbs;
}
}
}
}
else
{
for( int j=0; j<width; ++j )
{
for( int i=j+1; i<height; ++i )
{
const R alphaAbs = Abs(A.Get(i,j));
if( alphaAbs != 0 )
{
if( alphaAbs <= scale )
{
const R relScale = alphaAbs/scale;
scaledSquare += 2*relScale*relScale;
}
else
{
const R relScale = scale/alphaAbs;
scaledSquare = scaledSquare*relScale*relScale + 2;
scale = alphaAbs;
}
}
}
const R alphaAbs = Abs(A.Get(j,j));
if( alphaAbs != 0 )
{
if( alphaAbs <= scale )
{
const R relScale = alphaAbs/scale;
scaledSquare += relScale*relScale;
}
else
{
const R relScale = scale/alphaAbs;
scaledSquare = scaledSquare*relScale*relScale + 1;
scale = alphaAbs;
}
}
}
}
const R norm = scale*Sqrt(scaledSquare);
#ifndef RELEASE
PopCallStack();
#endif
return norm;
}
/** For reference, see http://realtimecollisiondetection.net/blog/?p=20 . */
Sphere Sphere::OptimalEnclosingSphere(const vec &a, const vec &b, const vec &c)
{
Sphere sphere;
vec ab = b-a;
vec ac = c-a;
float s, t;
bool areCollinear = ab.Cross(ac).LengthSq() < 1e-4f; // Manually test that we don't try to fit sphere to three collinear points.
bool success = !areCollinear && FitSphereThroughPoints(ab, ac, s, t);
if (!success || Abs(s) > 10000.f || Abs(t) > 10000.f) // If s and t are very far from the triangle, do a manual box fitting for numerical stability.
{
vec minPt = Min(a, b, c);
vec maxPt = Max(a, b, c);
sphere.pos = (minPt + maxPt) * 0.5f;
sphere.r = sphere.pos.Distance(minPt);
}
else if (s < 0.f)
{
sphere.pos = (a + c) * 0.5f;
sphere.r = a.Distance(c) * 0.5f;
sphere.r = Max(sphere.r, b.Distance(sphere.pos)); // For numerical stability, expand the radius of the sphere so it certainly contains the third point.
}
else if (t < 0.f)
{
sphere.pos = (a + b) * 0.5f;
sphere.r = a.Distance(b) * 0.5f;
sphere.r = Max(sphere.r, c.Distance(sphere.pos)); // For numerical stability, expand the radius of the sphere so it certainly contains the third point.
}
else if (s+t > 1.f)
{
sphere.pos = (b + c) * 0.5f;
sphere.r = b.Distance(c) * 0.5f;
sphere.r = Max(sphere.r, a.Distance(sphere.pos)); // For numerical stability, expand the radius of the sphere so it certainly contains the third point.
}
else
{
const vec center = s*ab + t*ac;
sphere.pos = a + center;
// Mathematically, the following would be correct, but it suffers from floating point inaccuracies,
// since it only tests distance against one point.
//sphere.r = center.Length();
// For robustness, take the radius to be the distance to the farthest point (though the distance are all
// equal).
sphere.r = Sqrt(Max(sphere.pos.DistanceSq(a), sphere.pos.DistanceSq(b), sphere.pos.DistanceSq(c)));
}
// Allow floating point inconsistency and expand the radius by a small epsilon so that the containment tests
// really contain the points (note that the points must be sufficiently near enough to the origin)
sphere.r += 2.f * sEpsilon; // We test against one epsilon, so expand by two epsilons.
#ifdef MATH_ASSERT_CORRECTNESS
if (!sphere.Contains(a, sEpsilon) || !sphere.Contains(b, sEpsilon) || !sphere.Contains(c, sEpsilon))
{
LOGE("Pos: %s, r: %f", sphere.pos.ToString().c_str(), sphere.r);
LOGE("A: %s, dist: %f", a.ToString().c_str(), a.Distance(sphere.pos));
LOGE("B: %s, dist: %f", b.ToString().c_str(), b.Distance(sphere.pos));
LOGE("C: %s, dist: %f", c.ToString().c_str(), c.Distance(sphere.pos));
mathassert(false);
}
#endif
return sphere;
}
This file is part of Elemental and is under the BSD 2-Clause License,
which can be found in the LICENSE file in the root directory, or at
http://opensource.org/licenses/BSD-2-Clause
*/
#include "El.hpp"
namespace El {
template<typename F>
void Fiedler( Matrix<F>& A, const vector<F>& c )
{
DEBUG_ONLY(CSE cse("Fiedler"))
const Int n = c.size();
A.Resize( n, n );
auto fiedlerFill = [&]( Int i, Int j ) { return Abs(c[i]-c[j]); };
IndexDependentFill( A, function<F(Int,Int)>(fiedlerFill) );
}
template<typename F>
void Fiedler( AbstractDistMatrix<F>& A, const vector<F>& c )
{
DEBUG_ONLY(CSE cse("Fiedler"))
const Int n = c.size();
A.Resize( n, n );
auto fiedlerFill = [&]( Int i, Int j ) { return Abs(c[i]-c[j]); };
IndexDependentFill( A, function<F(Int,Int)>(fiedlerFill) );
}
template<typename F>
void Fiedler( AbstractBlockDistMatrix<F>& A, const vector<F>& c )
void FoxLi( ElementalMatrix<Complex<Real>>& APre, Int n, Real omega )
{
DEBUG_CSE
typedef Complex<Real> C;
const Real pi = 4*Atan( Real(1) );
const C phi = Sqrt( C(0,omega/pi) );
DistMatrixWriteProxy<C,C,MC,MR> AProx( APre );
auto& A = AProx.Get();
// Compute Gauss quadrature points and weights
const Grid& g = A.Grid();
DistMatrix<Real,VR,STAR> d(g), e(g);
Zeros( d, n, 1 );
e.Resize( n-1, 1 );
auto& eLoc = e.Matrix();
for( Int iLoc=0; iLoc<e.LocalHeight(); ++iLoc )
{
const Int i = e.GlobalRow(iLoc);
const Real betaInv = 2*Sqrt(1-Pow(i+Real(1),-2)/4);
eLoc(iLoc) = 1/betaInv;
}
DistMatrix<Real,VR,STAR> x(g);
DistMatrix<Real,STAR,VR> Z(g);
HermitianTridiagEig( d, e, x, Z, UNSORTED );
auto z = Z( IR(0), ALL );
DistMatrix<Real,STAR,VR> sqrtWeights( z );
auto& sqrtWeightsLoc = sqrtWeights.Matrix();
for( Int jLoc=0; jLoc<sqrtWeights.LocalWidth(); ++jLoc )
sqrtWeightsLoc(0,jLoc) = Sqrt(Real(2))*Abs(sqrtWeightsLoc(0,jLoc));
herm_eig::Sort( x, sqrtWeights, ASCENDING );
// Form the integral operator
A.Resize( n, n );
DistMatrix<Real,MC,STAR> x_MC( A.Grid() );
DistMatrix<Real,MR,STAR> x_MR( A.Grid() );
x_MC.AlignWith( A );
x_MR.AlignWith( A );
x_MC = x;
x_MR = x;
auto& ALoc = A.Matrix();
auto& x_MCLoc = x_MC.Matrix();
auto& x_MRLoc = x_MR.Matrix();
for( Int jLoc=0; jLoc<A.LocalWidth(); ++jLoc )
{
for( Int iLoc=0; iLoc<A.LocalHeight(); ++iLoc )
{
const Real diff = x_MCLoc(iLoc)-x_MRLoc(jLoc);
const Real theta = -omega*Pow(diff,2);
const Real realPart = Cos(theta);
const Real imagPart = Sin(theta);
ALoc(iLoc,jLoc) = phi*C(realPart,imagPart);
}
}
// Apply the weighting
DistMatrix<Real,VR,STAR> sqrtWeightsTrans(g);
Transpose( sqrtWeights, sqrtWeightsTrans );
DiagonalScale( LEFT, NORMAL, sqrtWeightsTrans, A );
DiagonalScale( RIGHT, NORMAL, sqrtWeightsTrans, A );
}
static bool Compare
( const IndexValuePair<R>& a, const IndexValuePair<R>& b )
{ return Abs(a.value) < Abs(b.value); }
TWxTestResult WxTest(const TVector<double>& baseline,
const TVector<double>& test) {
TVector<double> diffs;
for (ui32 i = 0; i < baseline.size(); i++) {
const double i1 = baseline[i];
const double i2 = test[i];
const double diff = i1 - i2;
if (diff != 0) {
diffs.push_back(diff);
}
}
if (diffs.size() < 2) {
TWxTestResult result;
result.PValue = 0.5;
result.WMinus = result.WPlus = 0;
return result;
}
Sort(diffs.begin(), diffs.end(), [&](double x, double y) {
return Abs(x) < Abs(y);
});
double w_plus = 0;
double w_minus = 0;
double n = diffs.size();
for (int i = 0; i < n; ++i) {
double sum = 0;
double weight = 0;
int j = i;
double signPlus = 0;
double signMinus = 0;
for (j = i; j < n && diffs[j] == diffs[i]; ++j) {
sum += (j + 1);
++weight;
signPlus += diffs[i] >= 0;
signMinus += diffs[i] < 0;
}
const double meanRank = sum / weight;
w_plus += signPlus * meanRank;
w_minus += signMinus * meanRank;
i = j - 1;
}
TWxTestResult result;
result.WPlus = w_plus;
result.WMinus = w_minus;
const double w = result.WPlus - result.WMinus;
if (n > 16) {
double z = w / sqrt(n * (n + 1) * (2 * n + 1) * 1.0 / 6);
result.PValue = 2 * (1.0 - NormalCDF(Abs(z)));
} else {
result.PValue = 2 * CalcLevelOfSignificanceWXMPSR(Abs(w), (int) n);
}
result.PValue = 1.0 - result.PValue;
return result;
}
/*
** The GiST PickSplit method for _intments
** We use Guttman's poly time split algorithm
*/
Datum
g_int_picksplit(PG_FUNCTION_ARGS)
{
GistEntryVector *entryvec = (GistEntryVector *) PG_GETARG_POINTER(0);
GIST_SPLITVEC *v = (GIST_SPLITVEC *) PG_GETARG_POINTER(1);
OffsetNumber i,
j;
ArrayType *datum_alpha,
*datum_beta;
ArrayType *datum_l,
*datum_r;
ArrayType *union_d,
*union_dl,
*union_dr;
ArrayType *inter_d;
bool firsttime;
float size_alpha,
size_beta,
size_union,
size_inter;
float size_waste,
waste;
float size_l,
size_r;
int nbytes;
OffsetNumber seed_1 = 0,
seed_2 = 0;
OffsetNumber *left,
*right;
OffsetNumber maxoff;
SPLITCOST *costvector;
#ifdef GIST_DEBUG
elog(DEBUG3, "--------picksplit %d", entryvec->n);
#endif
maxoff = entryvec->n - 2;
nbytes = (maxoff + 2) * sizeof(OffsetNumber);
v->spl_left = (OffsetNumber *) palloc(nbytes);
v->spl_right = (OffsetNumber *) palloc(nbytes);
firsttime = true;
waste = 0.0;
for (i = FirstOffsetNumber; i < maxoff; i = OffsetNumberNext(i))
{
datum_alpha = GETENTRY(entryvec, i);
for (j = OffsetNumberNext(i); j <= maxoff; j = OffsetNumberNext(j))
{
datum_beta = GETENTRY(entryvec, j);
/* compute the wasted space by unioning these guys */
/* size_waste = size_union - size_inter; */
union_d = inner_int_union(datum_alpha, datum_beta);
rt__int_size(union_d, &size_union);
inter_d = inner_int_inter(datum_alpha, datum_beta);
rt__int_size(inter_d, &size_inter);
size_waste = size_union - size_inter;
pfree(union_d);
pfree(inter_d);
/*
* are these a more promising split that what we've already seen?
*/
if (size_waste > waste || firsttime)
{
waste = size_waste;
seed_1 = i;
seed_2 = j;
firsttime = false;
}
}
}
left = v->spl_left;
v->spl_nleft = 0;
right = v->spl_right;
v->spl_nright = 0;
if (seed_1 == 0 || seed_2 == 0)
{
seed_1 = 1;
seed_2 = 2;
}
datum_alpha = GETENTRY(entryvec, seed_1);
datum_l = copy_intArrayType(datum_alpha);
rt__int_size(datum_l, &size_l);
datum_beta = GETENTRY(entryvec, seed_2);
datum_r = copy_intArrayType(datum_beta);
rt__int_size(datum_r, &size_r);
maxoff = OffsetNumberNext(maxoff);
/*
* sort entries
*/
costvector = (SPLITCOST *) palloc(sizeof(SPLITCOST) * maxoff);
for (i = FirstOffsetNumber; i <= maxoff; i = OffsetNumberNext(i))
{
costvector[i - 1].pos = i;
datum_alpha = GETENTRY(entryvec, i);
union_d = inner_int_union(datum_l, datum_alpha);
rt__int_size(union_d, &size_alpha);
pfree(union_d);
union_d = inner_int_union(datum_r, datum_alpha);
rt__int_size(union_d, &size_beta);
pfree(union_d);
costvector[i - 1].cost = Abs((size_alpha - size_l) - (size_beta - size_r));
}
qsort((void *) costvector, maxoff, sizeof(SPLITCOST), comparecost);
/*
* Now split up the regions between the two seeds. An important property
* of this split algorithm is that the split vector v has the indices of
* items to be split in order in its left and right vectors. We exploit
* this property by doing a merge in the code that actually splits the
* page.
*
* For efficiency, we also place the new index tuple in this loop. This is
* handled at the very end, when we have placed all the existing tuples
* and i == maxoff + 1.
*/
for (j = 0; j < maxoff; j++)
{
i = costvector[j].pos;
/*
* If we've already decided where to place this item, just put it on
* the right list. Otherwise, we need to figure out which page needs
* the least enlargement in order to store the item.
*/
if (i == seed_1)
{
*left++ = i;
v->spl_nleft++;
continue;
}
else if (i == seed_2)
{
*right++ = i;
v->spl_nright++;
continue;
}
/* okay, which page needs least enlargement? */
datum_alpha = GETENTRY(entryvec, i);
union_dl = inner_int_union(datum_l, datum_alpha);
union_dr = inner_int_union(datum_r, datum_alpha);
rt__int_size(union_dl, &size_alpha);
rt__int_size(union_dr, &size_beta);
/* pick which page to add it to */
if (size_alpha - size_l < size_beta - size_r + WISH_F(v->spl_nleft, v->spl_nright, 0.01))
{
pfree(datum_l);
pfree(union_dr);
datum_l = union_dl;
size_l = size_alpha;
*left++ = i;
v->spl_nleft++;
}
else
{
pfree(datum_r);
pfree(union_dl);
datum_r = union_dr;
size_r = size_beta;
*right++ = i;
v->spl_nright++;
}
}
pfree(costvector);
*right = *left = FirstOffsetNumber;
v->spl_ldatum = PointerGetDatum(datum_l);
v->spl_rdatum = PointerGetDatum(datum_r);
PG_RETURN_POINTER(v);
}
inline BOOL EpsilonEq(const Type &a, const Type &b) { return Abs(a-b)<=BSP_EPSILON; };
Quaternion CreateDiagonalizer(Mat3Param matrix)
{
const unsigned cMaxSteps = 50;
const float cThetaLimit = 1.0e6f;
Quaternion quat(0.0f, 0.0f, 0.0f, 1.0f);
Matrix3 quatMatrix;
Matrix3 diagMatrix;
for(unsigned i = 0; i < cMaxSteps; ++i)
{
ToMatrix3(quat, &quatMatrix);
diagMatrix = Concat(Concat(quatMatrix, matrix), quatMatrix.Transposed());
//Elements not on the diagonal
Vector3 offDiag(diagMatrix(1, 2), diagMatrix(0, 2), diagMatrix(0, 1));
//Magnitude of the off-diagonal elements
Vector3 magDiag = Abs(offDiag);
//Index of the largest element
unsigned k = ((magDiag.x > magDiag.y) && (magDiag.x > magDiag.z)) ? 0 :
((magDiag.y > magDiag.z) ? 1 : 2);
unsigned k1 = (k + 1) % 3;
unsigned k2 = (k + 2) % 3;
//Diagonal already
if(offDiag[k] == 0.0f)
{
break;
}
float theta = (diagMatrix(k2, k2) - diagMatrix(k1, k1)) /
(2.0f * offDiag[k]);
float sign = Math::GetSign(theta);
//Make theta positive
theta *= sign;
//Large term in T
float thetaTerm = theta < 1e6f ? Math::Sqrt(Math::Sq(theta) + 1.0f)
: theta;
//Sign(T) / (|T| + sqrt(T^2 + 1))
float t = sign / (theta + thetaTerm);
//c = 1 / (t^2 + 1) t = s / c
float c = 1.0f / Math::Sqrt(Math::Sq(t) + 1.0f);
//No room for improvement - reached machine precision.
if(c == 1.0f)
{
break;
}
//Jacobi rotation for this iteration
Quaternion jacobi(0.0f, 0.0f, 0.0f, 0.0f);
//Using 1/2 angle identity sin(a/2) = sqrt((1-cos(a))/2)
jacobi[k] = sign * Math::Sqrt((1.0f - c) / 2.0f);
//Since our quat-to-matrix convention was for v*M instead of M*v
jacobi.w = Math::Sqrt(1.0f - Math::Sq(jacobi[k]));
//Reached limits of floating point precision
if(jacobi.w == 1.0f)
{
break;
}
quat *= jacobi;
Normalize(&quat);
}
return quat;
}
inline BOOL EpsilonNe(const Type &a, const Type &b) { return Abs(a-b)> BSP_EPSILON; };
/*
* 函数名: rt_thread_entry_handle
* 描 述: 手柄线程的入口函数
* 输 入: 无
* 输 出: 无
* 调 用: 内部调用
* 说 明:
*/
static void rt_thread_entry_handle(void* parameter)
{
double Handle_Speed_X;
double Handle_Speed_Y;
double Handle_Speed_Rotation;
struct Point temp_point;
struct Point now_point;
u32 temp,temp2;
u8 delay_flag = 1;
u8 hall_flag = 0;
u8 time_flag = 1,time_flag2 = 1;
u8 handleoff_flag = 1,handleoff_flag2 = 1;
u8 flag[6] = {0}; //对应电机的运行状态
int16_t Speed_M = 500;
float pos_openfan = 4;
extern float Motor2_Aim_Pos[];
extern u8 Motor2_Time_Flag;
u8 x_con_flag = 0; //X方向闭环
u8 XY_Only = 0;
Handle_Speed_X = 0;
Handle_Speed_Y = 0;
HandleData_X.lr= 12288;
HandleData_Y.lr= 12288;
TIM2->CNT = 0;
while(1)
{
temp = *handle_count;
temp2 = *handle_count2;
while(*handle_count == temp)
{
time_flag++;
Delay_ms(5);
if(time_flag > 60)//超时
{
time_flag = 0;
handle_timeout_flag = 1;
if(handleoff_flag)
{
Handle_Off_Count++;
handleoff_flag = 0;
}
break;
}
}
while(*handle_count2 == temp2)
{
time_flag2++;
Delay_ms(5);
if(time_flag2 > 60)//超时
{
time_flag2 = 0;
handle_timeout_flag2 = 1;
if(handleoff_flag2)
{
Handle_Off_Count2++;
handleoff_flag2 = 0;
}
break;
}
}
if(time_flag||time_flag2)//手柄工作正常
{
time_flag = 1;
time_flag2 = 1;
handle_timeout_flag = 0;
handle_timeout_flag2 = 0;
handleoff_flag = 1;
handleoff_flag2 = 1;
Handle_Speed_X = HandleData_X.lr-12288;
Handle_Speed_Y = HandleData_Y.lr-12288;
Handle_Speed_Rotation = 0;
//Handle_Speed_Y = HandleData_Y.ud-12288;
//Handle_Speed_Rotation = (HandleData_X.turn-12288)/10.0;
if(Button10_On && Button7_Off)
{
Handle_Speed_X /= 10.0;
Handle_Speed_Y /= 10.0;
Handle_Speed_Rotation /= 10.0;
Speed_M = 150;
}
else
Speed_M = 500;
if(Button9_On)
{
// HandoverFlag[0]=1;
// HandoverFlag[1]=1;
// HandoverFlag[2]=0;
// PoleFlag = 1;
// SwingFlag = 1;
}
if(Button9_Off)
{
// HandoverFlag[0]=0;
// HandoverFlag[1]=0;
// HandoverFlag[2]=0;
// PoleFlag = 0;
// SwingFlag = 0;
}
if(XY_Only)
{
// Handle_Speed_X=0;
Handle_Speed_Rotation=0;
}
if(GROUND==RED_GROUND)
{
Handle_Speed_X = -Handle_Speed_X;
Handle_Speed_Y = -Handle_Speed_Y;
}
if(x_con_flag)
{
temp_point.x = GPS.position.x + Handle_Speed_X*cos(GPS.radian);
temp_point.y = GPS.position.y + Handle_Speed_X*sin(GPS.radian);
SetSpeed(Speed_X,Speed_Y+Handle_Speed_Y,Speed_Rotation+Handle_Speed_Rotation);
EasyLine(temp_point,GPS.radian,Abs(Handle_Speed_X));
}
else
SetSpeed(Speed_X+Handle_Speed_X,Speed_Y+Handle_Speed_Y,Speed_Rotation+Handle_Speed_Rotation);
//SetSpeed(Speed_X,Speed_Y,Speed_Rotation+Handle_Speed_Rotation); //手柄前后出现问题，先无视之。。。
/*********************************上层动作控制***********************************/
//------------单键控制电机------------//
if(Button1_Up && Button7_Off && flag[1]==0)
{
flag[1]=1;
Moto_Set_GSpeed(W_MOTOR1_OLD_ID, 4*Speed_M);
}
if(Button1_Down && Button7_Off && flag[1]==0)
{
flag[1]=1;
Moto_Set_GSpeed(W_MOTOR1_OLD_ID, -4*Speed_M);
}
if(Button1_Off && (flag[1]==1))
{
flag[1]=0;
Moto_Stop(W_MOTOR1_OLD_ID);
}
if(Button2_Up && Button7_Off && flag[2]==0)
{
flag[2]=1;
Moto_Set_GSpeed(W_MOTOR2_OLD_ID, -3*Speed_M);
}
if(Button2_Down && Button7_Off && flag[2]==0)
{
flag[2]=1;
Moto_Set_GSpeed(W_MOTOR2_OLD_ID, 3*Speed_M);
}
if(Button2_Off && (flag[2]==1))
{
Moto_Stop(W_MOTOR2_OLD_ID);
flag[2]=0;
}
if(Button3_Up && Button7_Off && flag[3]==0)
{
flag[3]=1;
Moto_Set_GSpeed(W_MOTOR3_OLD_ID, -30*Speed_M);
}
if(Button3_Down && Button7_Off && flag[3]==0)
{
flag[3]=1;
Moto_Set_GSpeed(W_MOTOR3_OLD_ID, 30*Speed_M);
}
if(Button3_Off && (flag[3]==1))
{
flag[3]=0;
Moto_Stop(W_MOTOR3_OLD_ID);
}
//------------涵道------------//
if(Button4_Up && Button7_Off && Button10_Off)
{
Fan_Duty(L_CH,60.0);
Fan_Duty(R_CH,60.0);
}
if(Button4_Down && Button7_Off && Button10_Off)
{
Fan_Stop();
}
//------------霍尔------------//
if(Button5_Up && Button7_Off && Button10_Off && hall_flag == 0)
{
hall_flag = 1;
if(Hall_Count > 6) Hall_Count=1;
Hall_Send(Hall_Count);
}
if(Button5_Down && Button7_Off && Button10_Off && hall_flag == 0)
{
hall_flag = 1;
if(Hall_Count > 1) Hall_Count -= 2;
Hall_Send(Hall_Count);
}
if(Button5_Off && hall_flag == 1)
{
hall_flag = 0;
}
//------------双键组合用来控制电磁阀，7键为第二功能键------------//
if(Button6_Up && Button7_Off && Button10_Off){
HandPush();
}
if(Button6_Down && Button7_Off && Button10_Off){
HandPushOff();
}
//缓冲装置
if(Button1_Up && Button7_On && Button10_Off){
BufferOn();
}
if(Button1_Down && Button7_On && Button10_Off){
BufferOff();
}
//旋转调姿
if(Button2_Up && Button7_On && Button10_Off){
HandTurnRight();
}
if(Button2_Down && Button7_On && Button10_Off){
HandTurnLeft();
}
//俯仰调姿
if(Button3_Up && Button7_On && Button10_Off){
HandRaise();
}
if(Button3_Down && Button7_On && Button10_Off){
HandFall();
}
//放置大炮
if(Button4_Up && Button7_On && Button10_Off){
GunOn();
}
if(Button4_Down && Button7_On && Button10_Off){
GunOff();
}
//开炮
if(Button5_Up && Button7_On && Button10_Off){
FireOn();
}
if(Button5_Down && Button7_On && Button10_Off){
FireOff();
}
//交接爪
if(Button6_Up && Button7_On && Button10_Off){
HandOpen();
}
if(Button6_Down && Button7_On && Button10_Off){
HandClose();
}
//------------双键组合用来控制上下电机的位置，10键为第二功能键------------//
if(Button4_Up && Button7_Off && Button10_On){
Moto_Set_GPos(W_MOTOR2_OLD_ID,Motor2_Aim_Pos[0]);
}
if(Button4_Down && Button7_Off && Button10_On){
Moto_Set_GPos(W_MOTOR2_OLD_ID,Motor2_Aim_Pos[2]);
}
if(Button5_Up && Button7_Off && Button10_On){
Moto_Set_GPos(W_MOTOR2_OLD_ID,Motor2_Aim_Pos[4]);
}
if(Button5_Down && Button7_Off && Button10_On){
Moto_Set_GPos(W_MOTOR2_OLD_ID,Motor2_Aim_Pos[6]);
}
if(Button6_Up && Button7_Off && Button10_On){
HandUD(0,NO);
Delay_ms(500);
HandClose();//关爪子
HandFB(0,NO,3500);
FireOn();//开炮
Delay_ms(100);
now_point.x = GPS.position.x-700*cos(GPS.radian);
now_point.y = GPS.position.y-700*sin(GPS.radian);
SetLine(now_point,GPS.radian,100,1200,0);
while(1)
{
GoLine();
SetSpeed(Speed_X+Handle_Speed_X,Speed_Y+Handle_Speed_Y,Speed_Rotation+Handle_Speed_Rotation);
if(delay_flag)
delay_flag++;
if(delay_flag>100)
{
delay_flag = 0;
FireOff();
}
if(GetLength(GPS.position,now_point) < 10)
{
SPEED_STOP;
break;
}
Delay_ms(5);
}
delay_flag = 1;
FireClear();
Delay_ms(50);
BufferOn();//打开缓冲装置
Delay_ms(500);
BufferOff();//重新复位缓冲装置
}
if(Button6_Down && Button7_Off && Button10_On){
now_point.x = GPS.position.x+680*cos(GPS.radian);
now_point.y = GPS.position.y+680*sin(GPS.radian);
SetLine(now_point,GPS.radian,300,1200,0);
HandFB(Motor1_Aim_Pos[4],NO,3500);//抓取自动机器人
while(1)
{
GoLine();
SetSpeed(Speed_X+Handle_Speed_X,Speed_Y+Handle_Speed_Y,Speed_Rotation+Handle_Speed_Rotation);
if(GetLength(GPS.position,now_point) < 10)
{
SPEED_STOP;
break;
}
Delay_ms(5);
}
SetSpeed(Speed_X,Speed_Y,Speed_Rotation);
HandUD(0.5,NO);
HandOpen();//松开爪子
Delay_ms(300);
FireOff();
BufferOn();//打开缓冲装置
}
//三键组合用来重复任务
if(Button1_Up && Button7_On && Button10_On){
Retry = 1;
RouteFinish=0;
Hall_Send(1);//翘翘板初始化
rt_mb_send(&Mb_Emergency, 7);
rt_mb_send (&Mb_Arm, 1);
}
if(Button2_Up && Button7_On && Button10_On){
Retry = 1;
RouteFinish=0;
Hall_Send(2);//梅花桩初始化
rt_mb_send(&Mb_Emergency, 7);
rt_mb_send (&Mb_Arm, 4);
}
if(Button3_Up && Button7_On && Button10_On){
Retry = 1;
RouteFinish=0;
Hall_Send(4);//秋千初始化
rt_mb_send(&Mb_Emergency, 7);
rt_mb_send (&Mb_Arm, 6);
}
if(Button4_Up && Button7_On && Button10_On){
Retry = 1;
RouteFinish=0;
Hall_Send(6);//楼梯初始化
rt_mb_send(&Mb_Emergency, 7);
rt_mb_send (&Mb_Arm, 7);
}
if(Button5_Up && Button7_On && Button10_On){
rt_mb_send(&Mb_Emergency, 6);
}
if(Button6_Up && Button7_On && Button10_On){
Retry = 1;
RouteFinish=0;
rt_mb_send(&Mb_Emergency, 7);
//Motor_Init();
FireOff();
BufferOn();
ChooseGround_Pole(GROUND);
W_MOTOR_OLD_FUNC(W_MOTOR1_OLD_ID, 0.5 , 1000 , CMD_SET_PSG);
Moto_Set_GPos(W_MOTOR2_OLD_ID,2.55);
LCD_Clear();
LCD_SetXY(0,1);
LCD_Printf("Ready to go!");
Wait_Button8_Change();
rt_mb_send (&Mb_Auto, 1);
}
}
else
{
SPEED_STOP;
//Acc_Limit_Enable = 0;
SetSpeed(0,0,0);
//Acc_Limit_Enable = 1;
Moto_Stop(W_MOTOR1_OLD_ID);
Moto_Stop(W_MOTOR2_OLD_ID);
//Moto_Stop(W_MOTOR3_OLD_ID);
//Com_Printf(5,"v0\r");
}
if(Abs(Motor_Pos[0])>pos_openfan && CHILD_ON && !fan_flag)
{
fan_flag = 1;
Fan_Duty(L_CH,60.0);
Fan_Duty(R_CH,60.0);
}
if((Abs(Motor_Pos[0])<pos_openfan || CHILD_OFF) && fan_flag)
{
fan_flag = 0;
Fan_Stop();
}
if(Motor2_Time_Flag>10)
{
//W_MOTOR_OLD_FUNC(W_MOTOR2_OLD_ID, 0 , 0 , CMD_INIT_CAN);
Motor2_Time_Flag = 0;
}
Delay_ms(10);
}
}
TBool CT_ActiveRConsoleRead::KickStartL(const TDesC& aSection, const TInt aAsyncErrorIndex, RConsole& aConsole)
/**
* Kick Start the object and set up intials
* @param aSection The section in the ini containing data for the command
* @param aAsyncErrorIndex Command index for async calls to return errors to
* @param aConsole The RConsole object
*/
{
TBuf<KMaxTestExecuteCommandLength> tempStore;
iSection.Set(aSection);
iColourValueBlack =KBlack;
iColourValueWhite =KWhite;
iDataWrapperBase.GetUint8FromConfig(iSection, KFldColourBlack(), iColourValueBlack);
iDataWrapperBase.GetUint8FromConfig(iSection, KFldColourWhite(), iColourValueWhite);
iTimeOut=KDefaultTimeout;
iDataWrapperBase.GetIntFromConfig(iSection, KFldTimeout(), iTimeOut);
iErrorMargin=0;
iDataWrapperBase.GetIntFromConfig(iSection, KFldErrorMargin(), iErrorMargin);
iHasExitKeyCode=iDataWrapperBase.GetHexFromConfig(iSection, KFldExitKeyCode(), iExitKeyCode);
iHasExitRectangle=iDataWrapperBase.GetRectFromConfig(iSection, KFldExitRectangle(), iExitRectangle);
if ( iHasExitRectangle )
{
// Draw rectangle
TInt height =Abs(iExitRectangle.iBr.iY-iExitRectangle.iTl.iY);
TInt width =Abs(iExitRectangle.iBr.iX-iExitRectangle.iTl.iX);
CDrawUtils::DrawSquareUtility(iExitRectangle.iTl, height, width, iColourValueWhite);
}
iEvent.Reset();
TEventConfig config;
TInt eventIndex=0;
TBool dataOk=ETrue;
TBool moreData=ETrue;
while ( moreData )
{
tempStore.Format(KFldEventType, ++eventIndex);
moreData=iDataWrapperBase.GetEnumFromConfig(iSection, tempStore, iEnumRawEventTable, config.iEventType);
if ( moreData )
{
tempStore.Format(KFldEventOccurance, eventIndex);
TInt eventOccurance=EEventOccuranceOnce;
iDataWrapperBase.GetEnumFromConfig(iSection, tempStore, iEnumEventOccuranceTable, eventOccurance);
config.iEventOccurance=(TEventOccurance)eventOccurance;
tempStore.Format(KFldDataVerify, eventIndex);
config.iDataVerify=EFalse;
iDataWrapperBase.GetBoolFromConfig(iSection, tempStore, config.iDataVerify);
tempStore.Format(KFldDataDraw, eventIndex);
config.iDataDraw=EFalse;
iDataWrapperBase.GetBoolFromConfig(iSection, tempStore, config.iDataDraw);
iEvent.AppendL(config);
dataOk=ETrue;
}
}
// If -1 then we have an umlimited number of test(s) that completes with an exit event
// which can be an exit key code or a pen event in the exit rectangle
iNumberOfTests=-1;
iDataWrapperBase.GetIntFromConfig(iSection, KFldTests(), iNumberOfTests);
if ( dataOk )
{
iTestIndex=0;
dataOk=KickNext(aAsyncErrorIndex, aConsole);
}
return dataOk;
}
/** Initializes the XOSC32K crystal failure detector, and starts it.
*
* \param[in] ok_callback Callback function to run upon XOSC32K operational
* \param[in] fail_callback Callback function to run upon XOSC32K failure
*/
static void init_xosc32k_fail_detector(
const tc_callback_t ok_callback,
const tc_callback_t fail_callback)
{
/* TC pairs share the same clock, ensure reference and crystal timers use
* different clocks */
Assert(Abs(_tc_get_inst_index(CONF_TC_OSC32K) -
_tc_get_inst_index(CONF_TC_XOSC32K)) >= 2);
/* The crystal detection cycle count must be less than the reference cycle
* count, so that the reference timer is periodically reset before expiry */
Assert(CRYSTAL_RESET_CYCLES < CRYSTAL_FAIL_CYCLES);
/* Must use different clock generators for the crystal and reference, must
* not be CPU generator 0 */
Assert(GCLK_GENERATOR_XOSC32K != GCLK_GENERATOR_OSC32K);
Assert(GCLK_GENERATOR_XOSC32K != GCLK_GENERATOR_0);
Assert(GCLK_GENERATOR_OSC32K != GCLK_GENERATOR_0);
/* Configure and enable the XOSC32K GCLK generator */
struct system_gclk_gen_config xosc32k_gen_conf;
system_gclk_gen_get_config_defaults(&xosc32k_gen_conf);
xosc32k_gen_conf.source_clock = SYSTEM_CLOCK_SOURCE_XOSC32K;
system_gclk_gen_set_config(GCLK_GENERATOR_XOSC32K, &xosc32k_gen_conf);
system_gclk_gen_enable(GCLK_GENERATOR_XOSC32K);
/* Configure and enable the reference clock GCLK generator */
struct system_gclk_gen_config ref_gen_conf;
system_gclk_gen_get_config_defaults(&ref_gen_conf);
ref_gen_conf.source_clock = SYSTEM_CLOCK_SOURCE_OSC32K;
system_gclk_gen_set_config(GCLK_GENERATOR_OSC32K, &ref_gen_conf);
system_gclk_gen_enable(GCLK_GENERATOR_OSC32K);
/* Set up crystal counter - when target count elapses, trigger event */
struct tc_config tc_xosc32k_conf;
tc_get_config_defaults(&tc_xosc32k_conf);
tc_xosc32k_conf.clock_source = GCLK_GENERATOR_XOSC32K;
tc_xosc32k_conf.counter_16_bit.compare_capture_channel[0] =
CRYSTAL_RESET_CYCLES;
tc_xosc32k_conf.wave_generation = TC_WAVE_GENERATION_MATCH_FREQ;
tc_init(&tc_xosc32k, CONF_TC_XOSC32K, &tc_xosc32k_conf);
/* Set up reference counter - when event received, restart */
struct tc_config tc_osc32k_conf;
tc_get_config_defaults(&tc_osc32k_conf);
tc_osc32k_conf.clock_source = GCLK_GENERATOR_OSC32K;
tc_osc32k_conf.counter_16_bit.compare_capture_channel[0] =
CRYSTAL_FAIL_CYCLES;
tc_osc32k_conf.wave_generation = TC_WAVE_GENERATION_MATCH_FREQ;
tc_init(&tc_osc32k, CONF_TC_OSC32K, &tc_osc32k_conf);
/* Configure event channel and link it to the xosc32k counter */
struct events_config config;
struct events_resource event;
events_get_config_defaults(&config);
config.edge_detect = EVENTS_EDGE_DETECT_NONE;
config.generator = CONF_EVENT_GENERATOR_ID;
config.path = EVENTS_PATH_ASYNCHRONOUS;
events_allocate(&event, &config);
/* Configure event user and link it to the osc32k counter */
events_attach_user(&event, CONF_EVENT_USED_ID);
/* Enable event generation for crystal counter */
struct tc_events tc_xosc32k_events = { .generate_event_on_overflow = true };
tc_enable_events(&tc_xosc32k, &tc_xosc32k_events);
/* Enable event reception for reference counter */
struct tc_events tc_osc32k_events = { .on_event_perform_action = true };
tc_osc32k_events.event_action = TC_EVENT_ACTION_RETRIGGER;
tc_enable_events(&tc_osc32k, &tc_osc32k_events);
/* Enable overflow callback for the crystal counter - if crystal count
* has been reached, crystal is operational */
tc_register_callback(&tc_xosc32k, ok_callback, TC_CALLBACK_CC_CHANNEL0);
tc_enable_callback(&tc_xosc32k, TC_CALLBACK_CC_CHANNEL0);
/* Enable compare callback for the reference counter - if reference count
* has been reached, crystal has failed */
tc_register_callback(&tc_osc32k, fail_callback, TC_CALLBACK_CC_CHANNEL0);
tc_enable_callback(&tc_osc32k, TC_CALLBACK_CC_CHANNEL0);
/* Start both crystal and reference counters */
tc_enable(&tc_xosc32k);
tc_enable(&tc_osc32k);
}
/** Main application entry point. */
int main(void)
{
system_init();
system_flash_set_waitstates(2);
init_osc32k();
init_xosc32k();
init_xosc32k_fail_detector(
xosc32k_ok_callback, xosc32k_fail_callback);
#if ENABLE_CPU_CLOCK_OUT == true
/* Configure a GPIO pin as the CPU clock output */
struct system_pinmux_config clk_out_pin;
system_pinmux_get_config_defaults(&clk_out_pin);
clk_out_pin.direction = SYSTEM_PINMUX_PIN_DIR_OUTPUT;
clk_out_pin.mux_position = CONF_CLOCK_PIN_MUX;
system_pinmux_pin_set_config(CONF_CLOCK_PIN_OUT, &clk_out_pin);
#endif
for (;;) {
static bool old_run_osc = true;
bool new_run_osc =
(port_pin_get_input_level(BUTTON_0_PIN) == BUTTON_0_INACTIVE);
/* Check if the XOSC32K needs to be started or stopped when the board
* button is pressed or released */
if (new_run_osc != old_run_osc) {
if (new_run_osc) {
system_clock_source_enable(SYSTEM_CLOCK_SOURCE_XOSC32K);
while(!system_clock_source_is_ready(
SYSTEM_CLOCK_SOURCE_XOSC32K));
}
else {
system_clock_source_disable(SYSTEM_CLOCK_SOURCE_XOSC32K);
}
old_run_osc = new_run_osc;
}
}
}
int checkIrregFabCopy(const Box& a_domain)
{
int eekflag = 0;
int nvar = 1;
Interval interv(0,0);
int nghost= 0;
IntVect ivgh= IntVect::Zero;
Real tolerance = PolyGeom::getTolerance();
const EBIndexSpace* const ebisPtr = Chombo_EBIS::instance();
int numlevels = ebisPtr->numLevels();
Box levelDomain = a_domain;
for (int ilev = 0; ilev < numlevels-1; ilev++)
{
DisjointBoxLayout dblOne, dblTwo;
int maxsizeOne = 4;
int maxsizeTwo = 2;
makeLayout(dblOne, levelDomain, maxsizeOne);
makeLayout(dblTwo, levelDomain, maxsizeTwo);
EBISLayout ebislOne, ebislTwo;
makeEBISL(ebislOne, dblOne, levelDomain, nghost);
makeEBISL(ebislTwo, dblTwo, levelDomain, nghost);
LayoutData<IntVectSet> ldIVSOne(dblOne);
LayoutData<IntVectSet> ldIVSTwo(dblTwo);
for (DataIterator dit = dblOne.dataIterator(); dit.ok(); ++dit)
ldIVSOne[dit()] = IntVectSet(dblOne.get(dit()));
for (DataIterator dit = dblTwo.dataIterator(); dit.ok(); ++dit)
ldIVSTwo[dit()] = IntVectSet(dblTwo.get(dit()));
BaseIVFactory<Real> factOne(ebislOne, ldIVSOne);
BaseIVFactory<Real> factTwo(ebislTwo, ldIVSTwo);
LevelData<BaseIVFAB<Real> > dataOne(dblOne, nvar, ivgh, factOne);
LevelData<BaseIVFAB<Real> > dataTwo(dblTwo, nvar, ivgh, factTwo);
for (DataIterator dit = dblOne.dataIterator(); dit.ok(); ++dit)
dataOne[dit()].setVal(1);
for (DataIterator dit = dblTwo.dataIterator(); dit.ok(); ++dit)
dataTwo[dit()].setVal(2);
//copy dataone into datatwo. then all datatwo should hold are ones
dataOne.copyTo(interv, dataTwo, interv);
Real rightVal = 1.0;
for (DataIterator dit = dblTwo.dataIterator(); dit.ok(); ++dit)
{
const EBISBox& ebisBox = ebislTwo[dit()];
const IntVectSet& ivs = ldIVSTwo[dit()];
const BaseIVFAB<Real>& fab = dataTwo[dit()];
for (VoFIterator vofit(ivs, ebisBox.getEBGraph()); vofit.ok(); ++vofit)
{
Real thisVal = fab(vofit(), 0);
if (Abs(thisVal - rightVal) > tolerance)
{
eekflag = 3;
return eekflag;
}
}
}
levelDomain.coarsen(2);
}
return eekflag;
}