void FindFuncsLT(){
TGraph * FNegvsLT = new TGraph();
FNegvsLT->SetName("FNegvsLT");
double par[3] = {mtop, MW, mB};
for(int i = 0; i<1000; i++){
double x[4] = {1, 0, i*0.001, 0};
cout<<x[1]<<"\t"<<FNeg(x, par)<<endl;
FNegvsLT->SetPoint(i, (i*0.001), FNeg(x, par));
}
FNegvsLT->SaveAs("FNegvsLT.C");
TGraph * FPosvsLT = new TGraph();
FPosvsLT->SetName("FPosvsLT");
for(int i = 0; i<1000; i++){
double x[4] = {1, 0, i*0.001, 0};
cout<<x[1]<<"\t"<<FR(x, par)<<endl;
FPosvsLT->SetPoint(i, (i*0.001), FR(x, par));
}
FPosvsLT->SaveAs("FPosvsLT.C");
TGraph * FZerovsLT = new TGraph();
FZerovsLT->SetName("FZerovsLT");
for(int i = 0; i<1000; i++){
double x[4] = {1, 0, i*0.001, 0};
cout<<x[1]<<"\t"<<FZero(x, par)<<endl;
FZerovsLT->SetPoint(i, (i*0.001), FZero(x, par));
}
FZerovsLT->SaveAs("FZerovsLT.C");
}
void writeBackContactCoulomb(const PxcSolverConstraintDesc& desc, PxcSolverContext& cache,
PxcSolverBodyData& bd0, PxcSolverBodyData& bd1)
{
PxReal normalForce = 0;
FloatV normalForceV = FZero();
PxU8* PX_RESTRICT cPtr = desc.constraint;
PxReal* PX_RESTRICT vForceWriteback = reinterpret_cast<PxReal*>(desc.writeBack);
const PxcSolverContactCoulombHeader* PX_RESTRICT firstHeader = (const PxcSolverContactCoulombHeader*)cPtr;
PxU8* PX_RESTRICT last = desc.constraint + firstHeader->frictionOffset;
const PxU32 pointStride = firstHeader->type == PXS_SC_TYPE_EXT_CONTACT ? sizeof(PxcSolverContactExt)
: sizeof(PxcSolverContact);
bool hasForceThresholds = false;
while(cPtr < last)
{
const PxcSolverContactCoulombHeader* PX_RESTRICT hdr = (const PxcSolverContactCoulombHeader*)cPtr;
cPtr += sizeof(PxcSolverContactCoulombHeader);
hasForceThresholds = hdr->flags & PxcSolverContactHeader::eHAS_FORCE_THRESHOLDS;
const PxU32 numNormalConstr = hdr->numNormalConstr;
Ps::prefetchLine(cPtr, 256);
Ps::prefetchLine(cPtr, 384);
if(vForceWriteback!=NULL)
{
for(PxU32 i=0; i<numNormalConstr; i++)
{
PxcSolverContact* c = reinterpret_cast<PxcSolverContact*>(cPtr);
cPtr += pointStride;
const FloatV appliedForce = c->getAppliedForce();
FStore(appliedForce, vForceWriteback);
vForceWriteback++;
normalForceV = FAdd(normalForceV, appliedForce);
}
}
else
cPtr += numNormalConstr * pointStride;
}
PX_ASSERT(cPtr == last);
if(hasForceThresholds && desc.linkIndexA == PxcSolverConstraintDesc::NO_LINK && desc.linkIndexB == PxcSolverConstraintDesc::NO_LINK &&
normalForce !=0 && (bd0.reportThreshold < PX_MAX_REAL || bd1.reportThreshold < PX_MAX_REAL))
{
PxcThresholdStreamElement elt;
FStore(normalForceV, &elt.normalForce);
elt.threshold = PxMin<float>(bd0.reportThreshold, bd1.reportThreshold);
elt.body0 = bd0.originalBody;
elt.body1 = bd1.originalBody;
Ps::order(elt.body0,elt.body1);
PX_ASSERT(elt.body0 < elt.body1);
PX_ASSERT(cache.mThresholdStreamIndex<cache.mThresholdStreamLength);
cache.mThresholdStream[cache.mThresholdStreamIndex++] = elt;
}
}
void concludeContactCoulomb(const PxcSolverConstraintDesc& desc, PxcSolverContext& /*cache*/)
{
PxU8* PX_RESTRICT cPtr = desc.constraint;
const FloatV zero = FZero();
const PxcSolverContactCoulombHeader* PX_RESTRICT firstHeader = (const PxcSolverContactCoulombHeader*)cPtr;
PxU8* PX_RESTRICT last = desc.constraint + firstHeader->frictionOffset;//getConstraintLength(desc);
while(cPtr < last)
{
const PxcSolverContactCoulombHeader* PX_RESTRICT hdr = (const PxcSolverContactCoulombHeader*)cPtr;
cPtr += sizeof(PxcSolverContactCoulombHeader);
const PxU32 numNormalConstr = hdr->numNormalConstr;
//if(cPtr < last)
//Ps::prefetchLine(cPtr, 512);
Ps::prefetchLine(cPtr,128);
Ps::prefetchLine(cPtr,256);
Ps::prefetchLine(cPtr,384);
const PxU32 pointStride = hdr->type == PXS_SC_TYPE_EXT_CONTACT ? sizeof(PxcSolverContactExt)
: sizeof(PxcSolverContact);
for(PxU32 i=0;i<numNormalConstr;i++)
{
PxcSolverContact *c = reinterpret_cast<PxcSolverContact*>(cPtr);
cPtr += pointStride;
c->setScaledBias(FMax(c->getScaledBias(), zero));
}
}
PX_ASSERT(cPtr == last);
}
void CBotNav::Update()
{
RecordWaterState(); // record if bot needs to find air
// if bot is supposed to move, record if bot is stuck
if (m_pBot->GetMaxspeed() > 10 &&
(m_pBot->bi.actionflags & (ACTION_MOVEFORWARD | ACTION_MOVEBACK | ACTION_MOVELEFT | ACTION_MOVERIGHT))) {
// is it the time to check if bot is stuck?
if (FZero(m_flStuckCheckTime)) {
// first time, just record bot's current location
m_flStuckCheckTime = g_pServer->GetTime() + 2;
m_vecPrevLocation = m_pBot->GetOrigin();
m_flIsStuck = 0;
} else if (m_flStuckCheckTime < g_pServer->GetTime()) {
m_flStuckCheckTime = g_pServer->GetTime() + 0.5; // next stuck checking time
// if bot is on ground, check 2D Vector only; else check 3D Vector
float flBotMoved;
if (m_pBot->IsOnFloor()) {
flBotMoved = (m_vecPrevLocation - m_pBot->GetOrigin()).LengthSquared2D();
} else {
flBotMoved = (m_vecPrevLocation - m_pBot->GetOrigin()).LengthSquared();
}
// Did we NOT move enough previously?
if (flBotMoved < SQUARE(2)) {
m_flIsStuck += 0.3; // consider being stuck if so
} else if (m_pBot->IsOnFloor()) {
m_flIsStuck -= 0.2 * m_pBot->GetSpeed2D() / m_pBot->GetMaxspeed();
} else {
m_flIsStuck -= 0.2 * m_pBot->GetSpeed() / m_pBot->GetMaxspeed();
}
if (m_flIsStuck > 1.0)
m_flIsStuck = 1.0;
else if (m_flIsStuck < 0)
m_flIsStuck = 0;
m_vecPrevLocation = m_pBot->GetOrigin(); // record bot's current location
if (m_fPrevIsStuck && !IsStuck()) {
m_pBot->DebugMsg(DEBUG_BOTNAV, "Bot is UNSTUCK");
} else if (!m_fPrevIsStuck && IsStuck()) {
m_pBot->DebugMsg(DEBUG_BOTNAV, "Bot is STUCK!!!");
}
m_fPrevIsStuck = IsStuck();
}
} else {
// bot is supposed not to move
m_vecPrevLocation = NULLVEC;
m_flStuckCheckTime = 0;
m_flIsStuck = 0;
}
}
bool setupFinalizeExtSolverConstraintsCoulomb(PxcNpWorkUnit& n,
const ContactBuffer& buffer,
const PxcCorrelationBufferCoulomb& c,
const PxTransform& bodyFrame0,
const PxTransform& bodyFrame1,
bool /*perPointFriction*/,
PxU8* workspace,
PxReal invDt,
PxReal bounceThreshold,
PxsSolverExtBody& b0,
PxsSolverExtBody& b1,
PxU32 frictionCountPerPoint,
PxReal invMassScale0, PxReal invInertiaScale0,
PxReal invMassScale1, PxReal invInertiaScale1)
{
// NOTE II: the friction patches are sparse (some of them have no contact patches, and
// therefore did not get written back to the cache) but the patch addresses are dense,
// corresponding to valid patches
PxU8* PX_RESTRICT ptr = workspace;
const FloatV zero=FZero();
//KS - TODO - this should all be done in SIMD to avoid LHS
const PxF32 maxPenBias0 = b0.mLinkIndex == PxcSolverConstraintDesc::NO_LINK ? b0.mBodyData->penBiasClamp : getMaxPenBias(*b0.mFsData)[b0.mLinkIndex];
const PxF32 maxPenBias1 = b1.mLinkIndex == PxcSolverConstraintDesc::NO_LINK ? b1.mBodyData->penBiasClamp : getMaxPenBias(*b1.mFsData)[b0.mLinkIndex];
const FloatV maxPen = FLoad(PxMax(maxPenBias0, maxPenBias1)/invDt);
const FloatV restDistance = FLoad(n.restDistance);
Ps::prefetchLine(c.contactID);
Ps::prefetchLine(c.contactID, 128);
bool useExtContacts = (n.flags & (PxcNpWorkUnitFlag::eARTICULATION_BODY0|PxcNpWorkUnitFlag::eARTICULATION_BODY1))!=0;
const PxU32 frictionPatchCount = c.frictionPatchCount;
const bool staticBody = ((n.flags & PxcNpWorkUnitFlag::eDYNAMIC_BODY1) == 0);
const PxU32 pointStride = useExtContacts ? sizeof(PxcSolverContactExt) : sizeof(PxcSolverContact);
const PxU32 frictionStride = useExtContacts ? sizeof(PxcSolverFrictionExt) : sizeof(PxcSolverFriction);
const PxU8 pointHeaderType = Ps::to8(useExtContacts ? PXS_SC_TYPE_EXT_CONTACT : (staticBody ? PXS_SC_TYPE_STATIC_CONTACT : PXS_SC_TYPE_RB_CONTACT));
const PxU8 frictionHeaderType = Ps::to8(useExtContacts ? PXS_SC_TYPE_EXT_FRICTION : (staticBody ? PXS_SC_TYPE_STATIC_FRICTION : PXS_SC_TYPE_FRICTION));
PxReal d0 = n.dominance0 * invMassScale0;
PxReal d1 = n.dominance1 * invMassScale1;
PxReal angD0 = n.dominance0 * invInertiaScale0;
PxReal angD1 = n.dominance1 * invInertiaScale1;
for(PxU32 i=0;i< frictionPatchCount;i++)
{
const PxU32 contactCount = c.frictionPatchContactCounts[i];
if(contactCount == 0)
continue;
const Gu::ContactPoint* contactBase0 = buffer.contacts + c.contactPatches[c.correlationListHeads[i]].start;
const PxcFrictionPatchCoulomb& frictionPatch = c.frictionPatches[i];
const Vec3V normalV = Ps::aos::V3LoadU(frictionPatch.normal);
const PxVec3 normal = frictionPatch.normal;
const PxReal combinedRestitution = contactBase0->restitution;
PxcSolverContactCoulombHeader* PX_RESTRICT header = reinterpret_cast<PxcSolverContactCoulombHeader*>(ptr);
ptr += sizeof(PxcSolverContactCoulombHeader);
Ps::prefetchLine(ptr, 128);
Ps::prefetchLine(ptr, 256);
Ps::prefetchLine(ptr, 384);
header->numNormalConstr = (PxU8)contactCount;
header->type = pointHeaderType;
header->setRestitution(combinedRestitution);
header->setDominance0(d0);
header->setDominance1(d1);
header->angDom0 = angD0;
header->angDom1 = angD1;
header->setNormal(normalV);
for(PxU32 patch=c.correlationListHeads[i];
patch!=PxcCorrelationBuffer::LIST_END;
patch = c.contactPatches[patch].next)
{
const PxU32 count = c.contactPatches[patch].count;
const Gu::ContactPoint* contactBase = buffer.contacts + c.contactPatches[patch].start;
PxU8* p = ptr;
for(PxU32 j=0;j<count;j++)
{
const Gu::ContactPoint& contact = contactBase[j];
PxcSolverContactExt* PX_RESTRICT solverContact = reinterpret_cast<PxcSolverContactExt*>(p);
p += pointStride;
const FloatV separation = FLoad(contact.separation);
PxVec3 ra = contact.point - bodyFrame0.p;
PxVec3 rb = contact.point - bodyFrame1.p;
Vec3V targetVel = V3LoadU(contact.targetVel);
const FloatV maxImpulse = FLoad(contact.maxImpulse);
solverContact->scaledBiasX_targetVelocityY_maxImpulseZ = V3Merge(FMax(maxPen, FSub(separation, restDistance)), V3Dot(normalV,targetVel), maxImpulse);
//TODO - should we do cross only in vector land and then store. Could cause a LHS but probably no worse than
//what we already have (probably has a LHS from converting from vector to scalar above)
const PxVec3 raXn = ra.cross(normal);
const PxVec3 rbXn = rb.cross(normal);
Cm::SpatialVector deltaV0, deltaV1;
PxReal unitResponse = getImpulseResponse(b0, Cm::SpatialVector(normal, raXn), deltaV0, d0, angD0,
b1, Cm::SpatialVector(-normal, -rbXn), deltaV1, d1, angD1);
const PxReal vrel = b0.projectVelocity(normal, raXn)
- b1.projectVelocity(normal, rbXn);
solverContact->raXnXYZ_appliedForceW = V4SetW(Vec4V_From_Vec3V(V3LoadU(raXn)), zero);
solverContact->rbXnXYZ_velMultiplierW = V4SetW(Vec4V_From_Vec3V(V3LoadU(rbXn)), zero);
completeContactPoint(*solverContact, unitResponse, vrel, invDt, header->restitution, bounceThreshold);
solverContact->setDeltaVA(deltaV0.linear, deltaV0.angular);
solverContact->setDeltaVB(deltaV1.linear, deltaV1.angular);
}
ptr = p;
}
}
//construct all the frictions
PxU8* PX_RESTRICT ptr2 = workspace;
const PxF32 orthoThreshold = 0.70710678f;
const PxF32 eps = 0.00001f;
bool hasFriction = false;
for(PxU32 i=0;i< frictionPatchCount;i++)
{
const PxU32 contactCount = c.frictionPatchContactCounts[i];
if(contactCount == 0)
continue;
PxcSolverContactCoulombHeader* header = reinterpret_cast<PxcSolverContactCoulombHeader*>(ptr2);
header->frictionOffset = PxU16(ptr - ptr2);
ptr2 += sizeof(PxcSolverContactCoulombHeader) + header->numNormalConstr * pointStride;
PxVec3 normal = c.frictionPatches[i].normal;
const Gu::ContactPoint* contactBase0 = buffer.contacts + c.contactPatches[c.correlationListHeads[i]].start;
const PxReal staticFriction = contactBase0->staticFriction;
const PxU32 disableStrongFriction = contactBase0->internalFaceIndex1 & PxMaterialFlag::eDISABLE_FRICTION;
const bool haveFriction = (disableStrongFriction == 0);
PxcSolverFrictionHeader* frictionHeader = (PxcSolverFrictionHeader*)ptr;
frictionHeader->numNormalConstr = Ps::to8(c.frictionPatchContactCounts[i]);
frictionHeader->numFrictionConstr = Ps::to8(haveFriction ? c.frictionPatches[i].numConstraints : 0);
ptr += sizeof(PxcSolverFrictionHeader);
ptr += frictionHeader->getAppliedForcePaddingSize(c.frictionPatchContactCounts[i]);
Ps::prefetchLine(ptr, 128);
Ps::prefetchLine(ptr, 256);
Ps::prefetchLine(ptr, 384);
const PxVec3 t0Fallback1(0.f, -normal.z, normal.y);
const PxVec3 t0Fallback2(-normal.y, normal.x, 0.f) ;
const PxVec3 tFallback1 = orthoThreshold > PxAbs(normal.x) ? t0Fallback1 : t0Fallback2;
const PxVec3 vrel = b0.getLinVel() - b1.getLinVel();
const PxVec3 t0_ = vrel - normal * (normal.dot(vrel));
const PxReal sqDist = t0_.dot(t0_);
const PxVec3 tDir0 = (sqDist > eps ? t0_: tFallback1).getNormalized();
const PxVec3 tDir1 = tDir0.cross(normal);
PxVec3 tFallback[2] = {tDir0, tDir1};
PxU32 ind = 0;
if(haveFriction)
{
hasFriction = true;
frictionHeader->setStaticFriction(staticFriction);
frictionHeader->setDominance0(n.dominance0);
frictionHeader->setDominance1(n.dominance1);
frictionHeader->angDom0 = angD0;
frictionHeader->angDom1 = angD1;
frictionHeader->type = frictionHeaderType;
PxU32 totalPatchContactCount = 0;
for(PxU32 patch=c.correlationListHeads[i];
patch!=PxcCorrelationBuffer::LIST_END;
patch = c.contactPatches[patch].next)
{
const PxU32 count = c.contactPatches[patch].count;
const PxU32 start = c.contactPatches[patch].start;
const Gu::ContactPoint* contactBase = buffer.contacts + start;
PxU8* p = ptr;
for(PxU32 j =0; j < count; j++)
{
const PxU32 contactId = totalPatchContactCount + j;
const Gu::ContactPoint& contact = contactBase[j];
const PxVec3 ra = contact.point - bodyFrame0.p;
const PxVec3 rb = contact.point - bodyFrame1.p;
for(PxU32 k = 0; k < frictionCountPerPoint; ++k)
{
PxcSolverFrictionExt* PX_RESTRICT f0 = reinterpret_cast<PxcSolverFrictionExt*>(p);
p += frictionStride;
f0->contactIndex = contactId;
PxVec3 t0 = tFallback[ind];
ind = 1 - ind;
PxVec3 raXn = ra.cross(t0);
PxVec3 rbXn = rb.cross(t0);
Cm::SpatialVector deltaV0, deltaV1;
PxReal unitResponse = getImpulseResponse(b0, Cm::SpatialVector(t0, raXn), deltaV0, d0, angD0,
b1, Cm::SpatialVector(-t0, -rbXn), deltaV1, d1, angD1);
f0->setVelMultiplier(FLoad(unitResponse>0.0f ? 1.f/unitResponse : 0.0f));
f0->setRaXn(raXn);
f0->setRbXn(rbXn);
f0->setNormal(t0);
f0->setAppliedForce(0.0f);
f0->setDeltaVA(deltaV0.linear, deltaV0.angular);
f0->setDeltaVB(deltaV1.linear, deltaV1.angular);
}
}
totalPatchContactCount += c.contactPatches[patch].count;
ptr = p;
}
}
}
//PX_ASSERT(ptr - workspace == n.solverConstraintSize);
return hasFriction;
}
int main(int argc, char **argv)
{
int i_file,i_scan,i_chan,i_bin,n_omit,status=0;
int NFirstTable, NumHDU, NDumps, TotDumps=0, hdutype;
// int OutChans;
int spk;
int got_bins=0, got_mjd1=0; //, zeroed_outprofs=0;
long NPtsProf=0, FirstNPtsProf=0;
float Weight=1.0, TotWeight=0.;
// int ProfSum=0;
double x, ptype;
double MJD_first=0., MJD_last=0., MJD_mid;
double IMJDMid, MJDSecsMid;
double SBase,Srms,Duty,SPeak,FinalMask[NBINMAX];
double OutFreq;
char ProgName[32];
char Outfile[128];
char Header[256];
struct ASPHdr *Hdr;
struct SubHdr Subhdr;
struct StdProfs *InProfile, OutProfile;
struct RunVars RunMode;
fitsfile **Fin;
FILE *Fout, *Fcheck;
Cmdline *Cmd;
/* Get command line variables */
Cmd = parseCmdline(argc, argv);
/* Normally use this somewhere, and not showOptionValues */
Cmd->tool = Cmd->tool;
strcpy(ProgName, argv[0]);
Fin = (fitsfile **)malloc(Cmd->InfileC*sizeof(fitsfile));
Hdr = (struct ASPHdr *)malloc(Cmd->InfileC*sizeof(struct ASPHdr));
/* Dynamically allocate RunMode variables */
if (AllocRunMode(&RunMode) < 0){
printf("Could not allocate RunMode structure. Exiting...\n");
exit(2);
}
strcpy(RunMode.Infile,Cmd->Infile);
// if(!zeroed_outprofs) {
/* if(Cmd->SortChansP){
OutChans=Hdr[0].obs.NChan;
}
else{ */
// OutChans=1;
// }
// OutProfile=(struct StdProfs *)malloc(OutChans*sizeof(struct StdProfs));
// TotWeight=(float *)malloc(OutChans*sizeof(float));
/* Zero out profiles */
// for(i_chan=0;i_chan<OutChans;i_chan++){
FZero(OutProfile.rstds,NBINMAX);
FZero(OutProfile.rstdq,NBINMAX);
FZero(OutProfile.rstdu,NBINMAX);
FZero(OutProfile.rstdv,NBINMAX);
// }
//zeroed_outprofs=1;
// }
/* Create an output file to check omissions if in vebose mode */
if (Cmd->VerboseP || Cmd->CheckOmitP){
if((Fcheck = fopen("check_omit.dat","w")) == 0)
{ printf("Cannot open %s. Exiting...\n",Outfile); exit(1); }
}
/* read in all input files and add each to the final profile */
/* read in all input file headers */
NPtsProf=0;
for (i_file=0;i_file<Cmd->InfileC;i_file++){
n_omit=0;
status=0;
if(fits_open_file(&Fin[i_file], Cmd->Infile[i_file], READONLY, &status)){
printf("Error opening FITS file %s !!!\n", Cmd->Infile[i_file]);
exit(1);
}
if(ReadASPHdr(&Hdr[i_file], Fin[i_file]) < 0){
printf("%s> Unable to read Header from file %s. Exiting...\n",
ProgName,Cmd->Infile[i_file]);
exit(1);
}
/* Write file name in verbose-mode omit check file */
if(Cmd->VerboseP || Cmd->CheckOmitP)
fprintf(Fcheck, "\n%s\n",Cmd->Infile[i_file]);
/* for now just check if all files have same number of channels */
/* if(Cmd->SortChansP)
if(i_file>0 && Hdr[i_file].obs.NChan!=Hdr[0].obs.NChan){
fprintf(stderr,"%s> Different numbers of channels in different files:\n\n",
ProgName);
fprintf(stderr,"%s: %d channels, %s: %d channels\n",
Cmd->Infile[0],Hdr[0].obs.NChan,
Cmd->Infile[i_file],Hdr[i_file].obs.NChan);
} */
/* now find the number of dumps in the file */
fits_get_num_hdus(Fin[i_file], &NumHDU, &status);
if(!strcmp(Hdr[i_file].gen.HdrVer,"Ver1.0")){
NDumps = NumHDU-3; /* the "3" is temporary, depending on how
many non-data tables we will be using */
}
else if(!strcmp(Hdr[i_file].gen.HdrVer,"Ver1.0.1")){
NDumps = (NumHDU-3)/2;
}
else{
fprintf(stderr,"%s> Do not recognize FITS file version in header.\n",
ProgName);
fprintf(stderr,"This header is %s. Exiting...\n",Hdr[i_file].gen.HdrVer);
exit(1);
}
printf("File %s:\n",Cmd->Infile[i_file]);
printf(" Number of channels: %d\n",Hdr[i_file].obs.NChan) ;
printf(" Number of dumps: %d\n",NDumps);
/* Move to the first data table HDU in the fits file */
if(!strcmp(Hdr[i_file].gen.HdrVer,"Ver1.0"))
fits_movnam_hdu(Fin[i_file], BINARY_TBL, "STOKES0", 0, &status);
else if (!strcmp(Hdr[i_file].gen.HdrVer,"Ver1.0.1"))
fits_movnam_hdu(Fin[i_file], ASCII_TBL, "DUMPREF0", 0, &status);
/* Get the current HDU number */
fits_get_hdu_num(Fin[i_file], &NFirstTable);
/* Set up Profile structure size */
for(i_scan=0;i_scan<NDumps;i_scan++){
InProfile=(struct StdProfs *)malloc(Hdr[i_file].obs.NChan*
sizeof(struct StdProfs));
/* move to next dump's data */
if(!strcmp(Hdr[i_file].gen.HdrVer,"Ver1.0")){
fits_movabs_hdu(Fin[i_file], NFirstTable+i_scan, &hdutype, &status);
}
else if(!strcmp(Hdr[i_file].gen.HdrVer,"Ver1.0.1")){
/* if we've reached the end of the FITS file then increase FileNo */
fits_movabs_hdu(Fin[i_file],NFirstTable+(i_scan%MAXDUMPS)*2+1,&hdutype,
&status);
fits_get_num_rows(Fin[i_file], &NPtsProf, &status);status=0;
fits_movrel_hdu(Fin[i_file], -1, NULL, &status);
}
/* IF not done so, use number of bins from first file to compare to
the rest of the files */
if(got_bins==0){
FirstNPtsProf=NPtsProf;
got_bins=1;
}
/********** FIX: SKIP THIS WITHOUT DOING THE OMIT THING ***********/
/********** AND DON'T ADD TO NUMBER COUNT ************/
if(NPtsProf != FirstNPtsProf) {
fprintf(stderr,"Warning: Skipping scan %d (%ld bins ",
i_scan,NPtsProf);
fprintf(stderr,"vs. %ld bins in others).\n",FirstNPtsProf);
n_omit += Hdr[i_file].obs.NChan;
}
else{
/* find NPtsProf */
ReadASPStokes(&Hdr[i_file], &Subhdr, Fin[i_file], NPtsProf,
InProfile, i_scan, Cmd->VerboseP);
/* Add this profile onto running output profile */
for(i_chan=0;i_chan<Hdr[i_file].obs.NChan;i_chan++){
/* Bad scans are zeroed so if summ of the profile is zero, it's
not to be used in summation */
// ProfSum = FSum(&InProfile[i_chan].rstds[0], NPtsProf);
// ProfSum = 0;
// ProfSum = ArrayZero(InProfile[i_chan].rstds, NPtsProf);
// if(ProfSum != 0.0) { // i.e. good data
/* Test that all bins in current profile are not zeroed */
if(!ArrayZero(InProfile[i_chan].rstds, NPtsProf)) { // i.e. good data
// if(InProfile[i_chan].rstds[0] > -99998.) { // i.e. good data
/* If first MJD has not been registered, then do so since this
would be the first non-omitted scan */
if (got_mjd1==0) {
MJD_first = (double)Hdr[i_file].obs.IMJDStart +
Subhdr.DumpMiddleSecs/86400.;
got_mjd1=1;
}
if (i_scan==NDumps-1) {
/* Just keep overwriting MJD_last every i_file -- that way we
ensure getting the last MJD of the FINAL non-omitted scan
used */
MJD_last = (double)Hdr[i_file].obs.IMJDStart +
Subhdr.DumpMiddleSecs/86400.;
}
/* Get SNR for each Profile if we want to use weighting;
otherwise weights will all be 1.0 */
if(Cmd->WeightP) {
Duty = DutyLookup(Hdr[i_file].target.PSRName);
BMask(InProfile[i_chan].rstds,&Hdr[i_file].redn.RNBinTimeDump,
&Duty,FinalMask);
Baseline(InProfile[i_chan].rstds,FinalMask,
&Hdr[i_file].redn.RNBinTimeDump,&SBase,&Srms);
SPeak = FindPeak(InProfile[i_chan].rstds,
&Hdr[i_file].redn.RNBinTimeDump,&spk);
InProfile[i_chan].SNR = SPeak*Srms;
// Weight = InProfile[i_chan].SNR;
Weight = Srms; // which is actually 1/RMS.
}
// printf("SNR %d = %lf\n",i_chan,InProfile[i_chan].SNR);
/* Need to figure out how to organize input channels to match
* output channels */
/* if(Cmd->SortChansP){
for(i_bin=0;i_bin<NPtsProf;i_bin++) {
OutProfile[i_chan].rstds[i_bin] +=
Weight*InProfile[i_chan].rstds[i_bin];
OutProfile[i_chan].rstdq[i_bin] +=
Weight*InProfile[i_chan].rstdq[i_bin];
OutProfile[i_chan].rstdu[i_bin] +=
Weight*InProfile[i_chan].rstdu[i_bin];
OutProfile[i_chan].rstdv[i_bin] +=
Weight*InProfile[i_chan].rstdv[i_bin];
}
}
else{ */
for(i_bin=0;i_bin<NPtsProf;i_bin++) {
OutProfile.rstds[i_bin] +=
Weight*InProfile[i_chan].rstds[i_bin];
OutProfile.rstdq[i_bin] +=
Weight*InProfile[i_chan].rstdq[i_bin];
OutProfile.rstdu[i_bin] +=
Weight*InProfile[i_chan].rstdu[i_bin];
OutProfile.rstdv[i_bin] +=
Weight*InProfile[i_chan].rstdv[i_bin];
// printf("%f\n",OutProfile[0].rstds[i_bin]);fflush(stdout);
}
// }
TotWeight += Weight; // for now keep at zero index
/* Print profile weights for each scan, for each channel */
if(RunMode.Verbose) {
if(i_chan==0) printf("Profile weights -- scan %d: \n ",i_scan);
printf("%6.2f ",Weight);
if(i_chan==Hdr[i_file].obs.NChan-1) printf("\n");fflush(stdout);
}
}
else {
n_omit++;
if(Cmd->VerboseP || Cmd->CheckOmitP){
fprintf(Fcheck, "%6d %.1lf\n",
i_scan,Hdr[i_file].obs.ChanFreq[i_chan]);
/* printf("File %d, Dump %d, Channel %d (%lf MHz) were found to be\n",
i_file,i_scan,i_chan,Hdr[i_file].obs.ChanFreq[i_chan]);
printf(" zeroed and so are not included.\n");fflush(stdout); */
}
}
// if(i_chan==0) {for(i=0;i<50;i++) printf("%lf ",OutProfile[0].rstds[i]);printf("\n\n");fflush(stdout);};
}
/*****************/
/* } */
} /* else from positive check on NPtsProf */
free(InProfile);
}
/****** maybe bring this inside the ELSE where entire scans aren't being omitted ******/
/* if (Cmd->SortChansP)
TotDumps += (NDumps - n_omit);
else */
TotDumps += (NDumps*Hdr[i_file].obs.NChan - n_omit);
//free(InProfile);
printf("Reading of file %s complete and successful.\n",
Cmd->Infile[i_file]);
printf("%d scans omitted.\n\n",n_omit);fflush(stdout);
}
if(Cmd->VerboseP || Cmd->CheckOmitP) fclose(Fcheck);
/* Appease the format of the MakePol routine by making up these RunMode
structure members */
strcpy(RunMode.Source,Hdr[0].target.PSRName);
RunMode.Verbose = Cmd->VerboseP;
RunMode.FlipPA = 0;
RunMode.NoBase = Cmd->NoBaseP;
/* divide out total number of dumps to get the average */
// for(i_chan=0;i_chan<OutChans;i_chan++){
printf("Totdumps = %d\n",TotDumps);
fflush(stdout);
for(i_bin=0;i_bin<NPtsProf;i_bin++) {
OutProfile.rstds[i_bin] /= TotWeight;
OutProfile.rstdq[i_bin] /= TotWeight;
OutProfile.rstdu[i_bin] /= TotWeight;
OutProfile.rstdv[i_bin] /= TotWeight;
}
MakePol(&RunMode, (int)NPtsProf, &OutProfile);
/* Open file for writing */
sprintf(Outfile,"AddProf.out");
/* now write the output ascii added profile */
if ((Fout = fopen(Outfile,"w")) == 0)
{ printf("Cannot open %s. Exiting...\n",Outfile); exit(1); }
/* take average MJD of first to last scan */
MJD_mid = (MJD_first + MJD_last)/2.;
IMJDMid = floor(MJD_mid);
MJDSecsMid = (MJD_mid - IMJDMid)*86400.;
printf("MJD_mid = %lf, IMJDMid = %lf, MJDSecsMid = %lf\n",MJD_mid,IMJDMid,MJDSecsMid);fflush(stdout);
/* to choose a channel to put in the header for now, ust use the average
of the first datafile's channels */
OutFreq=0.;
for(i_chan=0; i_chan<Hdr[0].obs.NChan; i_chan++){
OutFreq += Hdr[0].obs.ChanFreq[i_chan];
}
OutFreq /= Hdr[0].obs.NChan;
/* Create and print header line for output file(s) */
sprintf(Header,"# %.1f %.7f %.10f %ld %.3f %.3f %d %s %d %9s %.10f",
IMJDMid,
MJDSecsMid,
// Subhdr.DumpRefPeriod[i_chan],
0.,
(long)1, OutFreq,
// Hdr[0].obs.DM, Hdr[i_file].redn.RNBinTimeDump,
Hdr[0].obs.DM, (int)NPtsProf,
Hdr[0].obs.ObsvtyCode, 1, Hdr[0].target.PSRName,
0.);
// Subhdr.DumpRefPhase[i_chan]);
fprintf(Fout,"%s\n",Header);
for(i_bin=0;i_bin<NPtsProf;i_bin++) {
/* see how strong the linear polarization is */
x = OutProfile.stdlin[i_bin]*OutProfile.Srms;
ptype = 43.1;
if (x > 1.) ptype=43.2;
if (x > 2.) ptype=43.3;
if (x > 3.) ptype=43.4;
if (x > 4.) ptype=43.5;
if (x > 5.) ptype=43.6;
fprintf(Fout,"%5d%15.7f%15.7f%15.7f%15.7f%15.7f%15.7f%15.7f%6.1f\n",i_bin,
OutProfile.rstds[i_bin],OutProfile.rstdq[i_bin],
OutProfile.rstdu[i_bin],
OutProfile.rstdv[i_bin],
/* phi in degrees */
OutProfile.stdlin[i_bin],
OutProfile.stdphi[i_bin]*180.0/TWOPI,
OutProfile.stdphierr[i_bin]*180.0/TWOPI,ptype);
}
printf("Created output file %s\n",Outfile);
fclose(Fout);
// }
/* Write all this to file */
printf("\nCompleted successfully.\n\n");fflush(stdout);
exit(0);
}
void solveContactCoulomb_BStatic(const PxcSolverConstraintDesc& desc, PxcSolverContext& /*cache*/)
{
PxcSolverBody& b0 = *desc.bodyA;
Vec3V linVel0 = V3LoadA(b0.linearVelocity);
Vec3V angVel0 = V3LoadA(b0.angularVelocity);
PxcSolverContactCoulombHeader* firstHeader = (PxcSolverContactCoulombHeader*)desc.constraint;
const PxU8* PX_RESTRICT last = desc.constraint + firstHeader->frictionOffset;//getConstraintLength(desc);
//hopefully pointer aliasing doesn't bite.
const PxU8* PX_RESTRICT currPtr = desc.constraint;
const FloatV zero = FZero();
while(currPtr < last)
{
PxcSolverContactCoulombHeader* PX_RESTRICT hdr = (PxcSolverContactCoulombHeader*)currPtr;
currPtr += sizeof(PxcSolverContactCoulombHeader);
const PxU32 numNormalConstr = hdr->numNormalConstr;
PxcSolverContact* PX_RESTRICT contacts = (PxcSolverContact*)currPtr;
Ps::prefetchLine(contacts);
currPtr += numNormalConstr * sizeof(PxcSolverContact);
PxF32* appliedImpulse = (PxF32*) (((PxU8*)hdr) + hdr->frictionOffset + sizeof(PxcSolverFrictionHeader));
Ps::prefetchLine(appliedImpulse);
const Vec3V normal = hdr->getNormal();
const FloatV invMassDom0 = FLoad(hdr->dominance0);
FloatV normalVel1 = V3Dot(normal, linVel0);
const Vec3V delLinVel0 = V3Scale(normal, invMassDom0);
FloatV accumDeltaF = zero;
//FloatV accumImpulse = zero;
for(PxU32 i=0;i<numNormalConstr;i++)
{
PxcSolverContact& c = contacts[i];
Ps::prefetchLine(&contacts[i+1]);
//const Vec4V normalXYZ_velMultiplierW = c.normalXYZ_velMultiplierW;
const Vec4V raXnXYZ_appliedForceW = c.raXnXYZ_appliedForceW;
const Vec4V rbXnXYZ_velMultiplierW = c.rbXnXYZ_velMultiplierW;
//const Vec3V normal = c.normal;
//const Vec3V normal = Vec3V_From_Vec4V(normalXYZ_velMultiplierW);
const Vec3V raXn = Vec3V_From_Vec4V(raXnXYZ_appliedForceW);
const FloatV appliedForce = V4GetW(raXnXYZ_appliedForceW);
const FloatV velMultiplier = V4GetW(rbXnXYZ_velMultiplierW);
//const FloatV velMultiplier = V4GetW(normalXYZ_velMultiplierW);
const Vec3V delAngVel0 = Vec3V_From_Vec4V(c.delAngVel0_InvMassADom);
const FloatV targetVel = c.getTargetVelocity();
const FloatV nScaledBias = FNeg(c.getScaledBias());
const FloatV maxImpulse = c.getMaxImpulse();
//Compute the normal velocity of the constraint.
//const FloatV normalVel1 = V3Dot(normal, linVel0);
const FloatV normalVel2 = V3Dot(raXn, angVel0);
const FloatV normalVel = FAdd(normalVel1, normalVel2);
//const FloatV unbiasedErr = FMul(targetVel, velMultiplier);
const FloatV biasedErr = FMulAdd(targetVel, velMultiplier, nScaledBias);
// still lots to do here: using loop pipelining we can interweave this code with the
// above - the code here has a lot of stalls that we would thereby eliminate
const FloatV _deltaF = FMax(FNegMulSub(normalVel, velMultiplier, biasedErr), FNeg(appliedForce));
const FloatV _newForce = FAdd(appliedForce, _deltaF);
const FloatV newForce = FMin(_newForce, maxImpulse);
const FloatV deltaF = FSub(newForce, appliedForce);
//linVel0 = V3MulAdd(delLinVel0, deltaF, linVel0);
normalVel1 = FScaleAdd(invMassDom0, deltaF, normalVel1);
angVel0 = V3ScaleAdd(delAngVel0, deltaF, angVel0);
accumDeltaF = FAdd(accumDeltaF, deltaF);
c.setAppliedForce(newForce);
Ps::aos::FStore(newForce, &appliedImpulse[i]);
Ps::prefetchLine(&appliedImpulse[i], 128);
//accumImpulse = FAdd(accumImpulse, newAppliedForce);
}
linVel0 = V3ScaleAdd(delLinVel0, accumDeltaF, linVel0);
//hdr->setAccumlatedForce(accumImpulse);
}
// Write back
V3StoreU(linVel0, b0.linearVelocity);
V3StoreU(angVel0, b0.angularVelocity);
PX_ASSERT(currPtr == last);
}
void setupFinalizeExtSolverContacts(
const ContactPoint* buffer,
const CorrelationBuffer& c,
const PxTransform& bodyFrame0,
const PxTransform& bodyFrame1,
PxU8* workspace,
const SolverExtBody& b0,
const SolverExtBody& b1,
const PxReal invDtF32,
PxReal bounceThresholdF32,
PxReal invMassScale0, PxReal invInertiaScale0,
PxReal invMassScale1, PxReal invInertiaScale1,
const PxReal restDist,
PxU8* frictionDataPtr,
PxReal ccdMaxContactDist)
{
// NOTE II: the friction patches are sparse (some of them have no contact patches, and
// therefore did not get written back to the cache) but the patch addresses are dense,
// corresponding to valid patches
/*const bool haveFriction = PX_IR(n.staticFriction) > 0 || PX_IR(n.dynamicFriction) > 0;*/
const FloatV ccdMaxSeparation = FLoad(ccdMaxContactDist);
PxU8* PX_RESTRICT ptr = workspace;
const FloatV zero=FZero();
//KS - TODO - this should all be done in SIMD to avoid LHS
const PxF32 maxPenBias0 = b0.mLinkIndex == PxSolverConstraintDesc::NO_LINK ? b0.mBodyData->penBiasClamp : getMaxPenBias(*b0.mFsData)[b0.mLinkIndex];
const PxF32 maxPenBias1 = b1.mLinkIndex == PxSolverConstraintDesc::NO_LINK ? b1.mBodyData->penBiasClamp : getMaxPenBias(*b1.mFsData)[b1.mLinkIndex];
const FloatV maxPenBias = FLoad(PxMax(maxPenBias0, maxPenBias1));
const PxReal d0 = invMassScale0;
const PxReal d1 = invMassScale1;
const PxReal angD0 = invInertiaScale0;
const PxReal angD1 = invInertiaScale1;
Vec4V staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W = V4Zero();
staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W=V4SetZ(staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W, FLoad(d0));
staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W=V4SetW(staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W, FLoad(d1));
const FloatV restDistance = FLoad(restDist);
PxU32 frictionPatchWritebackAddrIndex = 0;
PxU32 contactWritebackCount = 0;
Ps::prefetchLine(c.contactID);
Ps::prefetchLine(c.contactID, 128);
const FloatV invDt = FLoad(invDtF32);
const FloatV p8 = FLoad(0.8f);
const FloatV bounceThreshold = FLoad(bounceThresholdF32);
const FloatV invDtp8 = FMul(invDt, p8);
PxU8 flags = 0;
for(PxU32 i=0;i<c.frictionPatchCount;i++)
{
PxU32 contactCount = c.frictionPatchContactCounts[i];
if(contactCount == 0)
continue;
const FrictionPatch& frictionPatch = c.frictionPatches[i];
PX_ASSERT(frictionPatch.anchorCount <= 2); //0==anchorCount is allowed if all the contacts in the manifold have a large offset.
const Gu::ContactPoint* contactBase0 = buffer + c.contactPatches[c.correlationListHeads[i]].start;
const PxReal combinedRestitution = contactBase0->restitution;
const PxReal staticFriction = contactBase0->staticFriction;
const PxReal dynamicFriction = contactBase0->dynamicFriction;
const bool disableStrongFriction = !!(contactBase0->materialFlags & PxMaterialFlag::eDISABLE_FRICTION);
staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W=V4SetX(staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W, FLoad(staticFriction));
staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W=V4SetY(staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W, FLoad(dynamicFriction));
SolverContactHeader* PX_RESTRICT header = reinterpret_cast<SolverContactHeader*>(ptr);
ptr += sizeof(SolverContactHeader);
Ps::prefetchLine(ptr + 128);
Ps::prefetchLine(ptr + 256);
Ps::prefetchLine(ptr + 384);
const bool haveFriction = (disableStrongFriction == 0) ;//PX_IR(n.staticFriction) > 0 || PX_IR(n.dynamicFriction) > 0;
header->numNormalConstr = Ps::to8(contactCount);
header->numFrictionConstr = Ps::to8(haveFriction ? frictionPatch.anchorCount*2 : 0);
header->type = Ps::to8(DY_SC_TYPE_EXT_CONTACT);
header->flags = flags;
const FloatV restitution = FLoad(combinedRestitution);
header->staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W = staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W;
header->angDom0 = angD0;
header->angDom1 = angD1;
const PxU32 pointStride = sizeof(SolverContactPointExt);
const PxU32 frictionStride = sizeof(SolverContactFrictionExt);
const Vec3V normal = V3LoadU(buffer[c.contactPatches[c.correlationListHeads[i]].start].normal);
header->normal = normal;
for(PxU32 patch=c.correlationListHeads[i];
patch!=CorrelationBuffer::LIST_END;
patch = c.contactPatches[patch].next)
{
const PxU32 count = c.contactPatches[patch].count;
const Gu::ContactPoint* contactBase = buffer + c.contactPatches[patch].start;
PxU8* p = ptr;
for(PxU32 j=0;j<count;j++)
{
const Gu::ContactPoint& contact = contactBase[j];
SolverContactPointExt* PX_RESTRICT solverContact = reinterpret_cast<SolverContactPointExt*>(p);
p += pointStride;
setupExtSolverContact(b0, b1, d0, d1, angD0, angD1, bodyFrame0, bodyFrame1, normal, invDt, invDtp8, restDistance, maxPenBias, restitution,
bounceThreshold, contact, *solverContact, ccdMaxSeparation);
}
ptr = p;
}
contactWritebackCount += contactCount;
PxF32* forceBuffer = reinterpret_cast<PxF32*>(ptr);
PxMemZero(forceBuffer, sizeof(PxF32) * contactCount);
ptr += sizeof(PxF32) * ((contactCount + 3) & (~3));
header->broken = 0;
if(haveFriction)
{
//const Vec3V normal = Vec3V_From_PxVec3(buffer.contacts[c.contactPatches[c.correlationListHeads[i]].start].normal);
PxVec3 normalS = buffer[c.contactPatches[c.correlationListHeads[i]].start].normal;
PxVec3 t0, t1;
computeFrictionTangents(b0.getLinVel() - b1.getLinVel(), normalS, t0, t1);
Vec3V vT0 = V3LoadU(t0);
Vec3V vT1 = V3LoadU(t1);
//We want to set the writeBack ptr to point to the broken flag of the friction patch.
//On spu we have a slight problem here because the friction patch array is
//in local store rather than in main memory. The good news is that the address of the friction
//patch array in main memory is stored in the work unit. These two addresses will be equal
//except on spu where one is local store memory and the other is the effective address in main memory.
//Using the value stored in the work unit guarantees that the main memory address is used on all platforms.
PxU8* PX_RESTRICT writeback = frictionDataPtr + frictionPatchWritebackAddrIndex*sizeof(FrictionPatch);
header->frictionBrokenWritebackByte = writeback;
for(PxU32 j = 0; j < frictionPatch.anchorCount; j++)
{
SolverContactFrictionExt* PX_RESTRICT f0 = reinterpret_cast<SolverContactFrictionExt*>(ptr);
ptr += frictionStride;
SolverContactFrictionExt* PX_RESTRICT f1 = reinterpret_cast<SolverContactFrictionExt*>(ptr);
ptr += frictionStride;
PxVec3 ra = bodyFrame0.q.rotate(frictionPatch.body0Anchors[j]);
PxVec3 rb = bodyFrame1.q.rotate(frictionPatch.body1Anchors[j]);
PxVec3 error = (ra + bodyFrame0.p) - (rb + bodyFrame1.p);
{
const PxVec3 raXn = ra.cross(t0);
const PxVec3 rbXn = rb.cross(t0);
Cm::SpatialVector deltaV0, deltaV1;
const Cm::SpatialVector resp0 = createImpulseResponseVector(t0, raXn, b0);
const Cm::SpatialVector resp1 = createImpulseResponseVector(-t1, -rbXn, b1);
FloatV resp = FLoad(getImpulseResponse(b0, resp0, deltaV0, d0, angD0,
b1, resp1, deltaV1, d1, angD1));
const FloatV velMultiplier = FSel(FIsGrtr(resp, zero), FMul(p8, FRecip(resp)), zero);
PxU32 index = c.contactPatches[c.correlationListHeads[i]].start;
PxF32 targetVel = buffer[index].targetVel.dot(t0);
if(b0.mLinkIndex == PxSolverConstraintDesc::NO_LINK)
targetVel -= b0.projectVelocity(t0, raXn);
else if(b1.mLinkIndex == PxSolverConstraintDesc::NO_LINK)
targetVel += b1.projectVelocity(t0, rbXn);
f0->normalXYZ_appliedForceW = V4SetW(vT0, zero);
f0->raXnXYZ_velMultiplierW = V4SetW(V4LoadA(&resp0.angular.x), velMultiplier);
f0->rbXnXYZ_biasW = V4SetW(V4Neg(V4LoadA(&resp1.angular.x)), FLoad(t0.dot(error) * invDtF32));
f0->linDeltaVA = V3LoadA(deltaV0.linear);
f0->angDeltaVA = V3LoadA(deltaV0.angular);
f0->linDeltaVB = V3LoadA(deltaV1.linear);
f0->angDeltaVB = V3LoadA(deltaV1.angular);
f0->targetVel = targetVel;
}
{
const PxVec3 raXn = ra.cross(t1);
const PxVec3 rbXn = rb.cross(t1);
Cm::SpatialVector deltaV0, deltaV1;
const Cm::SpatialVector resp0 = createImpulseResponseVector(t1, raXn, b0);
const Cm::SpatialVector resp1 = createImpulseResponseVector(-t1, -rbXn, b1);
FloatV resp = FLoad(getImpulseResponse(b0, resp0, deltaV0, d0, angD0,
b1, resp1, deltaV1, d1, angD1));
const FloatV velMultiplier = FSel(FIsGrtr(resp, zero), FMul(p8, FRecip(resp)), zero);
PxU32 index = c.contactPatches[c.correlationListHeads[i]].start;
PxF32 targetVel = buffer[index].targetVel.dot(t0);
if(b0.mLinkIndex == PxSolverConstraintDesc::NO_LINK)
targetVel -= b0.projectVelocity(t1, raXn);
else if(b1.mLinkIndex == PxSolverConstraintDesc::NO_LINK)
targetVel += b1.projectVelocity(t1, rbXn);
f1->normalXYZ_appliedForceW = V4SetW(vT1, zero);
f1->raXnXYZ_velMultiplierW = V4SetW(V4LoadA(&resp0.angular.x), velMultiplier);
f1->rbXnXYZ_biasW = V4SetW(V4Neg(V4LoadA(&resp1.angular.x)), FLoad(t1.dot(error) * invDtF32));
f1->linDeltaVA = V3LoadA(deltaV0.linear);
f1->angDeltaVA = V3LoadA(deltaV0.angular);
f1->linDeltaVB = V3LoadA(deltaV1.linear);
f1->angDeltaVB = V3LoadA(deltaV1.angular);
f1->targetVel = targetVel;
}
}
}
frictionPatchWritebackAddrIndex++;
}
}
double FZeroLT(double * x, double * par){
double y[4] = {1, 0, x[0], 0};
return FZero(y,par);
}
bool pcmContactCapsuleConvex(GU_CONTACT_METHOD_ARGS)
{
PX_UNUSED(renderOutput);
const PxConvexMeshGeometryLL& shapeConvex = shape1.get<const PxConvexMeshGeometryLL>();
const PxCapsuleGeometry& shapeCapsule = shape0.get<const PxCapsuleGeometry>();
PersistentContactManifold& manifold = cache.getManifold();
Ps::prefetchLine(shapeConvex.hullData);
PX_ASSERT(transform1.q.isSane());
PX_ASSERT(transform0.q.isSane());
const Vec3V zeroV = V3Zero();
const Vec3V vScale = V3LoadU_SafeReadW(shapeConvex.scale.scale); // PT: safe because 'rotation' follows 'scale' in PxMeshScale
const FloatV contactDist = FLoad(params.mContactDistance);
const FloatV capsuleHalfHeight = FLoad(shapeCapsule.halfHeight);
const FloatV capsuleRadius = FLoad(shapeCapsule.radius);
const ConvexHullData* hullData =shapeConvex.hullData;
//Transfer A into the local space of B
const PsTransformV transf0 = loadTransformA(transform0);
const PsTransformV transf1 = loadTransformA(transform1);
const PsTransformV curRTrans(transf1.transformInv(transf0));
const PsMatTransformV aToB(curRTrans);
const FloatV convexMargin = Gu::CalculatePCMConvexMargin(hullData, vScale);
const FloatV capsuleMinMargin = Gu::CalculateCapsuleMinMargin(capsuleRadius);
const FloatV minMargin = FMin(convexMargin, capsuleMinMargin);
const PxU32 initialContacts = manifold.mNumContacts;
const FloatV projectBreakingThreshold = FMul(minMargin, FLoad(1.25f));
const FloatV refreshDist = FAdd(contactDist, capsuleRadius);
manifold.refreshContactPoints(aToB, projectBreakingThreshold, refreshDist);
//ML: after refreshContactPoints, we might lose some contacts
const bool bLostContacts = (manifold.mNumContacts != initialContacts);
GjkStatus status = manifold.mNumContacts > 0 ? GJK_UNDEFINED : GJK_NON_INTERSECT;
Vec3V closestA(zeroV), closestB(zeroV), normal(zeroV); // from a to b
const FloatV zero = FZero();
FloatV penDep = zero;
PX_UNUSED(bLostContacts);
if(bLostContacts || manifold.invalidate_SphereCapsule(curRTrans, minMargin))
{
const bool idtScale = shapeConvex.scale.isIdentity();
manifold.setRelativeTransform(curRTrans);
const QuatV vQuat = QuatVLoadU(&shapeConvex.scale.rotation.x);
ConvexHullV convexHull(hullData, zeroV, vScale, vQuat, idtScale);
convexHull.setMargin(zero);
//transform capsule(a) into the local space of convexHull(b)
CapsuleV capsule(aToB.p, aToB.rotate(V3Scale(V3UnitX(), capsuleHalfHeight)), capsuleRadius);
LocalConvex<CapsuleV> convexA(capsule);
const Vec3V initialSearchDir = V3Sub(capsule.getCenter(), convexHull.getCenter());
if(idtScale)
{
LocalConvex<ConvexHullNoScaleV> convexB(*PX_CONVEX_TO_NOSCALECONVEX(&convexHull));
status = gjkPenetration<LocalConvex<CapsuleV>, LocalConvex<ConvexHullNoScaleV> >(convexA, convexB, initialSearchDir, contactDist, closestA, closestB, normal, penDep,
manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, true);
}
else
{
LocalConvex<ConvexHullV> convexB(convexHull);
status = gjkPenetration<LocalConvex<CapsuleV>, LocalConvex<ConvexHullV> >(convexA, convexB, initialSearchDir, contactDist, closestA, closestB, normal, penDep,
manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, true);
}
Gu::PersistentContact* manifoldContacts = PX_CP_TO_PCP(contactBuffer.contacts);
bool doOverlapTest = false;
if(status == GJK_NON_INTERSECT)
{
return false;
}
else if(status == GJK_DEGENERATE)
{
return fullContactsGenerationCapsuleConvex(capsule, convexHull, aToB, transf0, transf1, manifoldContacts, contactBuffer, idtScale, manifold, normal,
closestB, convexHull.getMargin(), contactDist, true, renderOutput, FLoad(params.mToleranceLength));
}
else
{
const FloatV replaceBreakingThreshold = FMul(minMargin, FLoad(0.05f));
if(status == GJK_CONTACT)
{
const Vec3V localPointA = aToB.transformInv(closestA);//curRTrans.transformInv(closestA);
const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(normal), penDep);
//Add contact to contact stream
manifoldContacts[0].mLocalPointA = localPointA;
manifoldContacts[0].mLocalPointB = closestB;
manifoldContacts[0].mLocalNormalPen = localNormalPen;
//Add contact to manifold
manifold.addManifoldPoint2(localPointA, closestB, localNormalPen, replaceBreakingThreshold);
}
else
{
PX_ASSERT(status == EPA_CONTACT);
if(idtScale)
{
LocalConvex<ConvexHullNoScaleV> convexB(*PX_CONVEX_TO_NOSCALECONVEX(&convexHull));
status= Gu::epaPenetration(convexA, convexB, manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints,
closestA, closestB, normal, penDep, true);
}
else
{
LocalConvex<ConvexHullV> convexB(convexHull);
status= Gu::epaPenetration(convexA, convexB, manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints,
closestA, closestB, normal, penDep, true);
}
if(status == EPA_CONTACT)
{
const Vec3V localPointA = aToB.transformInv(closestA);//curRTrans.transformInv(closestA);
const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(normal), penDep);
//Add contact to contact stream
manifoldContacts[0].mLocalPointA = localPointA;
manifoldContacts[0].mLocalPointB = closestB;
manifoldContacts[0].mLocalNormalPen = localNormalPen;
//Add contact to manifold
manifold.addManifoldPoint2(localPointA, closestB, localNormalPen, replaceBreakingThreshold);
}
else
{
doOverlapTest = true;
}
}
if(initialContacts == 0 || bLostContacts || doOverlapTest)
{
return fullContactsGenerationCapsuleConvex(capsule, convexHull, aToB, transf0, transf1, manifoldContacts, contactBuffer, idtScale, manifold, normal,
closestB, convexHull.getMargin(), contactDist, doOverlapTest, renderOutput, FLoad(params.mToleranceLength));
}
else
{
//This contact is either come from GJK or EPA
normal = transf1.rotate(normal);
manifold.addManifoldContactsToContactBuffer(contactBuffer, normal, transf0, capsuleRadius, contactDist);
#if PCM_LOW_LEVEL_DEBUG
manifold.drawManifold(*renderOutput, transf0, transf1);
#endif
return true;
}
}
}
else if (manifold.getNumContacts() > 0)
{
normal = manifold.getWorldNormal(transf1);
manifold.addManifoldContactsToContactBuffer(contactBuffer, normal, transf0, capsuleRadius, contactDist);
#if PCM_LOW_LEVEL_DEBUG
manifold.drawManifold(*renderOutput, transf0, transf1);
#endif
return true;
}
return false;
}
