void PxFlyBall::addCPSS()
{
PxVec3 v = pxActor->getLinearVelocity();
PxTransform trans = pxActor->getGlobalPose();
PxVec3 pos = trans.p;
PxVec3 vn = v.getNormalized();
extern GLuint pCPS;
extern CircleParticleSys circleParticleSyses[CIRCLE_PARTICLESYS_NUM];
Color4f white(1.0, 1.0, 1.0, 1.0);
GLuint tgt = pCPS%CIRCLE_PARTICLESYS_NUM;
Vector3f pvn(-v.x, -v.y, -v.z);
circleParticleSyses[tgt].init(1.2,white, Vector3f(pos.x, pos.y, pos.z), pvn * 0.2, pvn * 0.4, 0, 3, 1, 2, 1.2, 1.4);
circleParticleSyses[tgt].initCircle(2,4, 1.8, 2, PI , PI / 2, 1.2, 1.2);
pCPS = pCPS + 1;
//pCPS = pCPS == CIRCLE_PARTICLESYS_NUM ? 0 : pCPS;
}
IK::Scalar IK::performFirstOrderStep(const Transformation& goalTransformation,IK::Scalar currentStepSize)
{
/*******************************************************************
Calculate chain of current global transformations from the root to
the leaf. The transformations are set up such that the leaf's
transformation when reaching the goal is the identity
transformation.
*******************************************************************/
/* Calculate joint transformations: */
calcJointTransformations();
/*******************************************************************
Calculate the joint parameter update vector and the current
residual.
*******************************************************************/
Scalar residual=calcDeltaP();
/*******************************************************************
If the effector joint is not the end effector (bi-directional IK),
the calculated parameter update vector must be modified to not move
the end effector. More precisely, the parameter update vector must
be projected into the full chain's Jacobian matrix' null space.
*******************************************************************/
if(effectorJointIndex<numJoints)
{
/*******************************************************************
Initialize the parameter update values for joints after the effector
to zero.
*******************************************************************/
for(int i=effectorJointIndex;i<numJoints;++i)
deltaP(i,0)=Scalar(0);
/*******************************************************************
Calculate the full chain's Jacobian matrix.
*******************************************************************/
calcJacobian();
/*******************************************************************
Project the calculated parameter update vector into the Jacobian's
null space. If IK were linear, this would avoid any motion of the
end effector.
*******************************************************************/
/* Calculate J * J^T: */
DenseMatrix jjt(6,6);
inplaceTransposed2Multiplication(jjt,jacobian,jacobian);
/* Calculate the parameter update vector's effect on the end effector: */
DenseMatrix jpv(6,1);
inplaceMultiplication(jpv,jacobian,deltaP);
/* First projection step: */
DenseMatrix p1=jjt.solveLinearEquations(jpv);
/* Second projection step: */
DenseMatrix pvn(numJoints,1);
inplaceTransposed1Multiplication(pvn,jacobian,p1);
/* Subtract pvn from deltaP to project deltaP into Jacobian's null space: */
for(int i=0;i<numJoints;++i)
deltaP(i,0)-=pvn(i,0);
}
/*******************************************************************
Perform a first-order integration step towards the goal using the
parameter update vector.
*******************************************************************/
updateJointAngles(currentStepSize);
return residual;
}
void scan_airspaces(const AircraftState state,
const Airspaces& airspaces,
const AirspaceAircraftPerformance& perf,
bool do_report,
const GeoPoint &target)
{
const fixed range(20000.0);
Directory::Create(_T("output/results"));
AirspaceVisitorPrint pvn("output/results/res-bb-nearest.txt",
do_report);
const Airspace *nearest = airspaces.FindNearest(state.location);
if (nearest != nullptr) {
AirspaceVisitor &v = pvn;
v.Visit(*nearest);
}
{
AirspaceVisitorPrint pvisitor("output/results/res-bb-range.txt",
do_report);
airspaces.VisitWithinRange(state.location, range, pvisitor);
}
{
AirspaceVisitorClosest pvisitor("output/results/res-bb-closest.txt",
airspaces.GetProjection(), state, perf);
airspaces.VisitWithinRange(state.location, range, pvisitor);
}
{
const std::vector<Airspace> vi = airspaces.FindInside(state);
AirspaceVisitorPrint pvi("output/results/res-bb-inside.txt",
do_report);
std::for_each(vi.begin(), vi.end(), CallVisitor<AirspaceVisitor>(pvi));
}
{
AirspaceIntersectionVisitorPrint ivisitor("output/results/res-bb-intersects.txt",
"output/results/res-bb-intersected.txt",
"output/results/res-bb-intercepts.txt",
do_report,
state, perf);
airspaces.VisitIntersecting(state.location, target, ivisitor);
}
{
AirspaceNearestSort ans(state.location);
const AbstractAirspace* as = ans.find_nearest(airspaces, range);
if (do_report) {
std::ofstream fout("output/results/res-bb-sortednearest.txt");
if (as) {
fout << *as << "\n";
} else {
fout << "# no nearest found\n";
}
}
}
{
AirspaceSoonestSort ans(state, perf);
const AbstractAirspace* as = ans.find_nearest(airspaces);
if (do_report) {
std::ofstream fout("output/results/res-bb-sortedsoonest.txt");
if (as) {
fout << *as << "\n";
} else {
fout << "# no soonest found\n";
}
}
}
}
void scan_airspaces(const AIRCRAFT_STATE state,
const Airspaces& airspaces,
const AirspaceAircraftPerformance& perf,
bool do_report,
const GeoPoint &target)
{
const fixed range(20000.0);
const std::vector<Airspace> vn = airspaces.scan_nearest(state.Location);
AirspaceVisitorPrint pvn("results/res-bb-nearest.txt",
do_report);
std::for_each(vn.begin(), vn.end(), CallVisitor<AirspaceVisitor>(pvn));
{
AirspaceVisitorPrint pvisitor("results/res-bb-range.txt",
do_report);
airspaces.visit_within_range(state.Location, range, pvisitor);
}
{
AirspaceVisitorClosest pvisitor("results/res-bb-closest.txt",
state, perf);
airspaces.visit_within_range(state.Location, range, pvisitor);
}
{
const std::vector<Airspace> vi = airspaces.find_inside(state);
AirspaceVisitorPrint pvi("results/res-bb-inside.txt",
do_report);
std::for_each(vi.begin(), vi.end(), CallVisitor<AirspaceVisitor>(pvi));
}
{
AirspaceIntersectionVisitorPrint ivisitor("results/res-bb-intersects.txt",
"results/res-bb-intersected.txt",
"results/res-bb-intercepts.txt",
do_report,
state, perf);
GeoVector vec(state.Location, target);
airspaces.visit_intersecting(state.Location, vec, ivisitor);
}
{
AirspaceNearestSort ans(state.Location);
const AbstractAirspace* as = ans.find_nearest(airspaces, range);
if (do_report) {
std::ofstream fout("results/res-bb-sortednearest.txt");
if (as) {
fout << *as << "\n";
} else {
fout << "# no nearest found\n";
}
}
}
{
AirspaceSoonestSort ans(state, perf);
const AbstractAirspace* as = ans.find_nearest(airspaces);
if (do_report) {
std::ofstream fout("results/res-bb-sortedsoonest.txt");
if (as) {
fout << *as << "\n";
} else {
fout << "# no soonest found\n";
}
}
}
}
