int main() {
__CPROVER_ASYNC_0: P0(0);
__CPROVER_ASYNC_1: P1(0);
__CPROVER_ASYNC_2: P2(0);
__CPROVER_assume(__unbuffered_cnt==3);
fence();
// EXPECT:exists
__CPROVER_assert(!(y==2 && __unbuffered_p1_r1==2 && __unbuffered_p2_r1==1 && __unbuffered_p2_r3==0), "Program was expected to be safe for PPC, model checker should have said NO.\nThis likely is a bug in the tool chain.");
return 0;
}
int main() {
__CPROVER_ASYNC_0: P0(0);
__CPROVER_ASYNC_1: P1(0);
__CPROVER_ASYNC_2: P2(0);
__CPROVER_assume(__unbuffered_cnt==3);
fence();
// EXPECT:exists
__CPROVER_assert(!(x==2 && y==2 && __unbuffered_p2_r3==0), "Program proven to be relaxed for PPC, model checker says YES.");
return 0;
}
AREXPORT ArMapObject::ArMapObject(const char *type,
ArPose pose,
const char *description,
const char *iconName,
const char *name,
bool hasFromTo,
ArPose fromPose,
ArPose toPose) :
myType((type != NULL) ? type : ""),
myBaseType(),
myName((name != NULL) ? name : "" ),
myDescription((description != NULL) ? description : "" ),
myPose(pose),
myIconName((iconName != NULL) ? iconName : "" ),
myHasFromTo(hasFromTo),
myFromPose(fromPose),
myToPose(toPose),
myFromToSegments(),
myStringRepresentation()
{
if (myHasFromTo)
{
double angle = myPose.getTh();
double sa = ArMath::sin(angle);
double ca = ArMath::cos(angle);
double fx = fromPose.getX();
double fy = fromPose.getY();
double tx = toPose.getX();
double ty = toPose.getY();
ArPose P0((fx*ca - fy*sa), (fx*sa + fy*ca));
ArPose P1((tx*ca - fy*sa), (tx*sa + fy*ca));
ArPose P2((tx*ca - ty*sa), (tx*sa + ty*ca));
ArPose P3((fx*ca - ty*sa), (fx*sa + ty*ca));
myFromToSegments.push_back(ArLineSegment(P0, P1));
myFromToSegments.push_back(ArLineSegment(P1, P2));
myFromToSegments.push_back(ArLineSegment(P2, P3));
myFromToSegments.push_back(ArLineSegment(P3, P0));
myFromToSegment.newEndPoints(fromPose, toPose);
}
else { // pose
size_t whPos = myType.rfind("WithHeading");
size_t whLen = 11;
if (whPos > 0) {
if (whPos == myType.size() - whLen) {
myBaseType = myType.substr(0, whPos);
}
}
} // end else pose
IFDEBUG(
ArLog::log(ArLog::Normal,
"ArMapObject::ctor() created %s (%s)",
myName.c_str(), myType.c_str());
);
int main() {
__CPROVER_ASYNC_0: P0(0);
__CPROVER_ASYNC_1: P1(0);
__CPROVER_ASYNC_2: P2(0);
__CPROVER_ASYNC_3: P3(0);
__CPROVER_assume(__unbuffered_cnt==4);
fence();
// EXPECT:exists
__CPROVER_assert(!(x==2 && z==2 && __unbuffered_p1_EAX==0 && __unbuffered_p3_EAX==2 && __unbuffered_p3_EBX==0), "Program proven to be relaxed for X86, model checker says YES.");
return 0;
}
PFilter::PFilter(std::size_t x_size, std::size_t s_size) :
Sample_state_filter (x_size, s_size),
Kalman_state_filter(x_size),
SIR_kalman_scheme(x_size, s_size, rnd),
w(wir)
{
PFilter::x_size = x_size;
FM::Vec x0(x_size);
x0.clear();
FM::SymMatrix P0(x_size, x_size);
P0.clear();
init(x0, P0);
}
void compress(sm3_data_picker& data){
boost::uint32_t A= ctx_.hash32[0];
boost::uint32_t B= ctx_.hash32[1];
boost::uint32_t C= ctx_.hash32[2];
boost::uint32_t D= ctx_.hash32[3];
boost::uint32_t E= ctx_.hash32[4];
boost::uint32_t F= ctx_.hash32[5];
boost::uint32_t G= ctx_.hash32[6];
boost::uint32_t H= ctx_.hash32[7];
boost::uint32_t SS1=0, SS2=0, TT1=0, TT2=0;
boost::uint32_t W[64+4]={0},W1[64]={0};
for (int i=0;i<16;++i){
W[i]=data.next_uint32_be();
}
for (int j = 16; j < 68; j++) {
W[j] = P1(W[j - 16] ^ W[j - 9] ^ circular_shift_l<boost::uint32_t>(W[j - 3],15)) ^ circular_shift_l<boost::uint32_t>(W[j - 13],7) ^ W[j - 6] ;
}
for (int j = 0; j < 64; j++) {
W1[j] = W[j] ^ W[j + 4];
}
for (int j = 0; j < 64; j++) {
SS1 =circular_shift_l<boost::uint32_t>(
(circular_shift_l<boost::uint32_t>(A,12) + E + circular_shift_l<boost::uint32_t>(get_magic_number(j),j) )
,7);
SS2 = SS1 ^ circular_shift_l<boost::uint32_t>(A,12);
TT1 = FF(A, B, C, j) + D + SS2 + W1[j];
unsigned long ggh=GG(E,F,G,j);
TT2 = GG(E, F, G, j) + H + SS1 + W[j];
D = C;
C = circular_shift_l<boost::uint32_t>(B,9);
B = A;
A = TT1;
H = G;
G = circular_shift_l<boost::uint32_t>(F,19);
F = E;
E = P0(TT2);
}
ctx_.hash32[0] ^=A;
ctx_.hash32[1] ^=B;
ctx_.hash32[2] ^=C;
ctx_.hash32[3] ^=D;
ctx_.hash32[4] ^=E;
ctx_.hash32[5] ^=F;
ctx_.hash32[6] ^=G;
ctx_.hash32[7] ^=H;
}
void PolynomialEpipolaireCoordinate::write(class ELISE_fp & aFile) const
{
aFile.write(P0());
aFile.write(DirX());
aFile.write(TrFin());
aFile.write(mPolToYEpip);
aFile.write(AmplInv());
aFile.write(DeltaDegre());
aFile.write(bool(mGridCor!=0));
if (mGridCor!=0)
mGridCor->write(aFile);
}
Point Rectangle::Sample(float u, float v, Normal *Ns) const {
// u and v are random samples on the surface of the light source
Point P0(-x/2, y/2, height), P1(x/2, y/2, height);
Point P2(-x/2, -y/2, height), P3(x/2, -y/2, height);
Point p = P0 + (P1 - P0) * u + (P2 - P0) * v;
Normal n = Normal(Cross(P2-P0, P1-P0));
*Ns = Normalize(n);
// NORMAL ON THE PLANE
*Ns = Normalize((*ObjectToWorld)(n));
if (ReverseOrientation) *Ns *= -1.f;
return (*ObjectToWorld)(p);
}
void srTSpherMirror::SetupInAndOutPlanes(TVector3d* InPlane, TVector3d* OutPlane)
{// Assumes InPlaneNorm and transverse part InPlaneCenPo already defined !!!
TVector3d &InPlaneCenPo = *InPlane, &InPlaneNorm = InPlane[1];
TVector3d &OutPlaneCenPo = *OutPlane, &OutPlaneNorm = OutPlane[1];
TVector3d LocInPlaneNorm = InPlaneNorm;
FromLabToLocFrame_Vector(LocInPlaneNorm);
double xP = -0.5*Dx, yP = -0.5*Dy;
TVector3d P0(xP, yP, SurfaceFunction(xP, yP, 0));
TVector3d P1(-xP, yP, SurfaceFunction(-xP, yP, 0));
TVector3d P2(xP, -yP, SurfaceFunction(xP, -yP, 0));
TVector3d P3(-xP, -yP, SurfaceFunction(-xP, -yP, 0));
TVector3d EdgePoints[] = { P0, P1, P2, P3 };
int LowestInd, UppestInd;
FindLowestAndUppestPoints(LocInPlaneNorm, EdgePoints, 4, LowestInd, UppestInd);
TVector3d PointForInPlane = EdgePoints[LowestInd];
FromLocToLabFrame_Point(PointForInPlane);
InPlaneCenPo.y = PointForInPlane.y;
TVector3d LocInPlaneCenPo = InPlaneCenPo;
FromLabToLocFrame_Point(LocInPlaneCenPo);
TVector3d LocInPlane[] = { LocInPlaneCenPo, LocInPlaneNorm };
TVector3d LocP, LocN;
FindRayIntersectWithSurface(LocInPlane, 0, LocP);
SurfaceNormalAtPoint(LocP.x, LocP.y, 0, LocN);
TVector3d LocOutPlaneNorm = LocInPlaneNorm;
ReflectVect(LocN, LocOutPlaneNorm);
FindLowestAndUppestPoints(LocOutPlaneNorm, EdgePoints, 4, LowestInd, UppestInd);
OutPlaneCenPo = (EdgePoints[UppestInd]*LocOutPlaneNorm)*LocOutPlaneNorm;
*OutPlaneInLocFrame = OutPlaneCenPo; OutPlaneInLocFrame[1] = LocOutPlaneNorm;
FromLocToLabFrame_Point(OutPlaneCenPo);
OutPlaneNorm = LocOutPlaneNorm;
FromLocToLabFrame_Vector(OutPlaneNorm);
TVector3d LabN = LocN;
FromLocToLabFrame_Vector(LabN);
ExRefInLabFrameBeforeProp = TVector3d(1.,0.,0.);
ReflectVect(LabN, ExRefInLabFrameBeforeProp);
EzRefInLabFrameBeforeProp = TVector3d(0.,0.,1.);
ReflectVect(LabN, EzRefInLabFrameBeforeProp);
}
void Centerline::buildKdTree()
{
FILE * f = Fopen("myPOINTS.pos","w");
fprintf(f, "View \"\"{\n");
int nbPL = 3; //10 points per line
//int nbNodes = (lines.size()+1) + (nbPL*lines.size());
int nbNodes = (colorp.size()) + (nbPL*lines.size());
ANNpointArray nodes = annAllocPts(nbNodes, 3);
int ind = 0;
std::map<MVertex*, int>::iterator itp = colorp.begin();
while (itp != colorp.end()){
MVertex *v = itp->first;
nodes[ind][0] = v->x();
nodes[ind][1] = v->y();
nodes[ind][2] = v->z();
itp++; ind++;
}
for(unsigned int k = 0; k < lines.size(); ++k){
MVertex *v0 = lines[k]->getVertex(0);
MVertex *v1 = lines[k]->getVertex(1);
SVector3 P0(v0->x(),v0->y(), v0->z());
SVector3 P1(v1->x(),v1->y(), v1->z());
for (int j= 1; j < nbPL+1; j++){
double inc = (double)j/(double)(nbPL+1);
SVector3 Pj = P0+inc*(P1-P0);
nodes[ind][0] = Pj.x();
nodes[ind][1] = Pj.y();
nodes[ind][2] = Pj.z();
ind++;
}
}
kdtree = new ANNkd_tree(nodes, nbNodes, 3);
for(int i = 0; i < nbNodes; ++i){
fprintf(f, "SP(%g,%g,%g){%g};\n",
nodes[i][0], nodes[i][1],nodes[i][2],1.0);
}
fprintf(f,"};\n");
fclose(f);
}
/**
* Returns the probability of static empty
*/
float getFreeStaticLikelihood()
{
if( (b_entry_event + b_exit_event)<20.0 )
{
return 0.5f;
}
Eigen::Matrix2f P;
Eigen::Vector2f P0;
Eigen::Vector2f u1(0.5, 0.5);
float Lex = exitL();
float Len = entryL();
P(0,0) = (1.0-Len);
P(0,1) = Len;
P(1,0) = Lex;
P(1,1) = (1-Lex);
P0 = u1.transpose() * P;
return (P0(0));
}
AREXPORT void ArForbiddenRangeDevice::processMap(void)
{
std::list<ArMapObject *>::const_iterator it;
ArMapObject *obj;
myDataMutex.lock();
ArUtil::deleteSet(mySegments.begin(), mySegments.end());
mySegments.clear();
for (it = myMap->getMapObjects()->begin();
it != myMap->getMapObjects()->end();
it++)
{
obj = (*it);
if (strcmp(obj->getType(), "ForbiddenLine") == 0 &&
obj->hasFromTo())
{
mySegments.push_back(new ArLineSegment(obj->getFromPose(),
obj->getToPose()));
}
if (strcmp(obj->getType(), "ForbiddenArea") == 0 &&
obj->hasFromTo())
{
double angle = obj->getPose().getTh();
double sa = ArMath::sin(angle);
double ca = ArMath::cos(angle);
double fx = obj->getFromPose().getX();
double fy = obj->getFromPose().getY();
double tx = obj->getToPose().getX();
double ty = obj->getToPose().getY();
ArPose P0((fx*ca - fy*sa), (fx*sa + fy*ca));
ArPose P1((tx*ca - fy*sa), (tx*sa + fy*ca));
ArPose P2((tx*ca - ty*sa), (tx*sa + ty*ca));
ArPose P3((fx*ca - ty*sa), (fx*sa + ty*ca));
mySegments.push_back(new ArLineSegment(P0, P1));
mySegments.push_back(new ArLineSegment(P1, P2));
mySegments.push_back(new ArLineSegment(P2, P3));
mySegments.push_back(new ArLineSegment(P3, P0));
}
}
myDataMutex.unlock();
}
int main(int argc, char* argv[]) {
srand(time(0));
point_pf::initialize();
Matrix<X_DIM> x0, xGoal;
x0[0] = -3.5; x0[1] = 2;
xGoal[0] = -3.5; xGoal[1] = -2;
SymmetricMatrix<X_DIM> Sigma0 = .01*identity<X_DIM>();
Matrix<U_DIM> u = (xGoal - x0) / (DT*(T-1));
std::vector<Matrix<X_DIM> > P0(M);
for(int m=0; m < M; ++m) {
P0[m] = sampleGaussian(x0, Sigma0);
}
std::vector<std::vector<Matrix<X_DIM>> > P_t(T);
std::vector<Matrix<Q_DIM> > dyn_noise;
std::vector<Matrix<R_DIM> > obs_noise;
float sampling_noise;
P_t[0] = P0;
for(int t=0; t < T-1; ++t) {
dyn_noise = sampleGaussianN(zeros<Q_DIM,1>(), Q, M);
obs_noise = sampleGaussianN(zeros<R_DIM,1>(), R, M);
sampling_noise = (1/float(M))*(rand() / float(RAND_MAX));
P_t[t+1] = point_pf::beliefDynamics(P_t[t], u, dyn_noise, obs_noise, sampling_noise);
//P_t[t+1] = point_pf::casadiBeliefDynamics(P_t[t], u, dyn_noise, obs_noise, sampling_noise);
}
for(int t=0; t < T; ++t) {
std::cout << "\nt: " << t << "\n";
for(int m=0; m < M; ++m) {
std::cout << ~P_t[t][m];
}
}
point_pf::pythonDisplayParticles(P_t);
return 0;
}
PolynomialEpipolaireCoordinate *
PolynomialEpipolaireCoordinate::PolMapingChScale
(REAL aChSacle) const
{
PolynomialEpipolaireCoordinate * aRes =
new PolynomialEpipolaireCoordinate
(
P0() * aChSacle,
DirX(),
mPolToYEpip.MapingChScale(aChSacle),
AmplInv() * aChSacle,
DeltaDegre(),
TrFin() * aChSacle
);
if (mGridCor)
aRes->mGridCor = mGridCor->NewChScale(aChSacle,true);
return aRes;
}
float Rectangle::Pdf(const Point &p, const Vector &wi) const
{
// Intersect sample ray with area light geometry
DifferentialGeometry dgLight;
Ray ray(p, wi, 1e-3f);
ray.depth = -1; // temporary hack to ignore alpha mask
float thit, rayEpsilon;
if (!Intersect(ray, &thit, &rayEpsilon, &dgLight)) return 0.;
float pdf;
Point pObject = (*WorldToObject)(p);
// TODO: Check with the normal (n.p)
if (!(pObject.x < x/2 && pObject.x > -x/2
&& pObject.y < y/2 && pObject.y > -y/2))
return 0;
// N & wi
Point P0(-x/2, y/2, height);
Point P1(x/2, y/2, height);
Point P2(-x/2, -y/2, height);
Point P0_W = (*WorldToObject)(P0);
Point P1_W = (*WorldToObject)(P1);
Point P2_W = (*WorldToObject)(P2);
Normal n = Normal(Cross(P2_W-P0_W, P1_W-P0_W));
Normal Ns = Normalize(n);
Normal Nwi(wi.x, wi.y, wi.z);
if (Dot(Ns, Nwi) < 0.0010)
pdf = 1.0;
if (isinf(pdf)) pdf = 0.f;
return pdf;
}
void ConstitutiveModelDriver<EvalT, Traits>::
evaluateFields(typename Traits::EvalData workset)
{
bool print = false;
if (typeid(ScalarT) == typeid(RealType)) print = true;
std::cout.precision(15);
std::cout << "ConstitutiveModelDriver<EvalT, Traits>::evaluateFields" << std::endl;
Intrepid2::Tensor<ScalarT> F(num_dims_), P(num_dims_), sig(num_dims_);
Intrepid2::Tensor<ScalarT> F0(num_dims_), P0(num_dims_);
for (int cell = 0; cell < workset.numCells; ++cell) {
for (int pt = 0; pt < num_pts_; ++pt) {
F0.fill(prescribed_def_grad_,cell,pt,0,0);
F.fill(def_grad_,cell,pt,0,0);
sig.fill(stress_,cell,pt,0,0);
P = Intrepid2::piola(F,sig);
if (print) {
std::cout << "F: \n" << F << std::endl;
std::cout << "P: \n" << P << std::endl;
std::cout << "sig: \n" << sig << std::endl;
}
for (int node = 0; node < num_nodes_; ++node) {
for (int dim1 = 0; dim1 < num_dims_; ++dim1) {
for (int dim2 = 0; dim2 < num_dims_; ++dim2) {
residual_(cell,node,dim1,dim2) =
(F(dim1,dim2) - F0(dim1,dim2));
//* (P(dim1,dim2) - P0(dim1,dim2));
}
}
}
}
}
}
int
/* main(int argc, char *argv[]) */
whetstone_main()
{
/* used in the FORTRAN version */
long I;
long N1, N2, N3, N4, N6, N7, N8, N9, N10, N11;
double X1,X2,X3,X4,X,Y,Z;
long LOOP;
int II, JJ;
/* added for this version */
long loopstart;
long long startsec, finisec;
float KIPS;
int continuous;
loopstart = 1000000; /* see the note about LOOP below */
loopstart = 250000;
continuous = 0;
II = 1; /* start at the first arg (temp use of II here) */
/* while (II < argc) { */
/* if (strncmp(argv[II], "-c", 2) == 0 || argv[II][0] == 'c') { */
/* continuous = 1; */
/* } else if (atol(argv[II]) > 0) { */
/* loopstart = atol(argv[II]); */
/* } else { */
/* fprintf(stderr, USAGE); */
/* return(1); */
/* } */
/* II++; */
/* } */
LCONT:
/*
C
C Start benchmark timing at this point.
C
*/
startsec = get_microsec();// time(0);
/*
C
C The actual benchmark starts here.
C
*/
T = .499975;
T1 = 0.50025;
T2 = 2.0;
/*
C
C With loopcount LOOP=10, one million Whetstone instructions
C will be executed in EACH MAJOR LOOP..A MAJOR LOOP IS EXECUTED
C 'II' TIMES TO INCREASE WALL-CLOCK TIMING ACCURACY.
C
LOOP = 1000;
*/
LOOP = loopstart;
II = 1;
JJ = 1;
IILOOP:
N1 = 0;
N2 = 12 * LOOP;
N3 = 14 * LOOP;
N4 = 345 * LOOP;
N6 = 210 * LOOP;
N7 = 32 * LOOP;
N8 = 899 * LOOP;
N9 = 616 * LOOP;
N10 = 0;
N11 = 93 * LOOP;
/*
C
C Module 1: Simple identifiers
C
*/
X1 = 1.0;
X2 = -1.0;
X3 = -1.0;
X4 = -1.0;
for (I = 1; I <= N1; I++) {
X1 = (X1 + X2 + X3 - X4) * T;
X2 = (X1 + X2 - X3 + X4) * T;
X3 = (X1 - X2 + X3 + X4) * T;
X4 = (-X1+ X2 + X3 + X4) * T;
}
#ifdef PRINTOUT
IF (JJ==II)POUT(N1,N1,N1,X1,X2,X3,X4);
#endif
/*
C
C Module 2: Array elements
C
*/
E1[1] = 1.0;
E1[2] = -1.0;
E1[3] = -1.0;
E1[4] = -1.0;
for (I = 1; I <= N2; I++) {
E1[1] = ( E1[1] + E1[2] + E1[3] - E1[4]) * T;
E1[2] = ( E1[1] + E1[2] - E1[3] + E1[4]) * T;
E1[3] = ( E1[1] - E1[2] + E1[3] + E1[4]) * T;
E1[4] = (-E1[1] + E1[2] + E1[3] + E1[4]) * T;
}
#ifdef PRINTOUT
IF (JJ==II)POUT(N2,N3,N2,E1[1],E1[2],E1[3],E1[4]);
#endif
/*
C
C Module 3: Array as parameter
C
*/
for (I = 1; I <= N3; I++)
PA(E1);
#ifdef PRINTOUT
IF (JJ==II)POUT(N3,N2,N2,E1[1],E1[2],E1[3],E1[4]);
#endif
/*
C
C Module 4: Conditional jumps
C
*/
J = 1;
for (I = 1; I <= N4; I++) {
if (J == 1)
J = 2;
else
J = 3;
if (J > 2)
J = 0;
else
J = 1;
if (J < 1)
J = 1;
else
J = 0;
}
#ifdef PRINTOUT
IF (JJ==II)POUT(N4,J,J,X1,X2,X3,X4);
#endif
/*
C
C Module 5: Omitted
C Module 6: Integer arithmetic
C
*/
J = 1;
K = 2;
L = 3;
for (I = 1; I <= N6; I++) {
J = J * (K-J) * (L-K);
K = L * K - (L-J) * K;
L = (L-K) * (K+J);
E1[L-1] = J + K + L;
E1[K-1] = J * K * L;
}
#ifdef PRINTOUT
IF (JJ==II)POUT(N6,J,K,E1[1],E1[2],E1[3],E1[4]);
#endif
/*
C
C Module 7: Trigonometric functions
C
*/
X = 0.5;
Y = 0.5;
for (I = 1; I <= N7; I++) {
X = T * DATAN(T2*DSIN(X)*DCOS(X)/(DCOS(X+Y)+DCOS(X-Y)-1.0));
Y = T * DATAN(T2*DSIN(Y)*DCOS(Y)/(DCOS(X+Y)+DCOS(X-Y)-1.0));
}
#ifdef PRINTOUT
IF (JJ==II)POUT(N7,J,K,X,X,Y,Y);
#endif
/*
C
C Module 8: Procedure calls
C
*/
X = 1.0;
Y = 1.0;
Z = 1.0;
for (I = 1; I <= N8; I++)
P3(X,Y,&Z);
#ifdef PRINTOUT
IF (JJ==II)POUT(N8,J,K,X,Y,Z,Z);
#endif
/*
C
C Module 9: Array references
C
*/
J = 1;
K = 2;
L = 3;
E1[1] = 1.0;
E1[2] = 2.0;
E1[3] = 3.0;
for (I = 1; I <= N9; I++)
P0();
#ifdef PRINTOUT
IF (JJ==II)POUT(N9,J,K,E1[1],E1[2],E1[3],E1[4]);
#endif
/*
C
C Module 10: Integer arithmetic
C
*/
J = 2;
K = 3;
for (I = 1; I <= N10; I++) {
J = J + K;
K = J + K;
J = K - J;
K = K - J - J;
}
#ifdef PRINTOUT
IF (JJ==II)POUT(N10,J,K,X1,X2,X3,X4);
#endif
/*
C
C Module 11: Standard functions
C
*/
X = 0.75;
for (I = 1; I <= N11; I++)
X = DSQRT(DEXP(DLOG(X)/T1));
#ifdef PRINTOUT
IF (JJ==II)POUT(N11,J,K,X,X,X,X);
#endif
/*
C
C THIS IS THE END OF THE MAJOR LOOP.
C
*/
if (++JJ <= II)
goto IILOOP;
/*
C
C Stop benchmark timing at this point.
C
*/
finisec = get_microsec();// time(0);
/*
C----------------------------------------------------------------
C Performance in Whetstone KIP's per second is given by
C
C (100*LOOP*II)/TIME
C
C where TIME is in seconds.
C--------------------------------------------------------------------
*/
_printf("\n");
if (finisec-startsec <= 0) {
_printf("Insufficient duration- Increase the LOOP count\n");
return(1);
}
_printf("Loops: %ld, Iterations: %d, Duration: %lld sec.\n",
LOOP, II, (finisec-startsec)/1000000);
KIPS = (100.0*LOOP*II)/((double)(finisec-startsec)/1000000.0); //(((double)(finisec-startsec)) / (double)CLOCKS_PER_SEC );
if (KIPS >= 1000.0){
_printf("C Converted Double Precision Whetstones: %.1f MIPS\n", KIPS/1000.0);
}
else{
_printf("C Converted Double Precision Whetstones: %.1f KIPS\n", KIPS);
}
if (continuous)
goto LCONT;
return(0);
}
void test_EKF_interface_simulated_data()
{
vcl_string panFile("/Users/jimmy/Data/pan_regression/real_data/RF_raw_output.txt");
vcl_string velocityFile("/Users/jimmy/Data/pan_regression/real_data/RF_raw_velocity.txt");
vcl_string panGdFile("/Users/jimmy/Data/pan_regression/real_data/quarter_2_spherical_map/testing_fn_pan_features.txt");
vnl_matrix<double> panMat;
vnl_matrix<double> velocityMat;
vnl_matrix<double> panGdMat;
VnlPlus::readMat(panFile.c_str(), panMat);
VnlPlus::readMat(velocityFile.c_str(), velocityMat);
VnlPlus::readMat(panGdFile.c_str(), panGdMat);
vcl_vector<double> observed_pan;
vcl_vector<double> observed_velocity;
for (int i = 0; i<panMat.rows(); i++) {
observed_pan.push_back(panMat(i, 1));
observed_velocity.push_back(velocityMat(i, 1));
}
vcl_vector<double> smoothed_pan;
vcl_vector<double> smoothed_velocity;
vnl_vector<double> x0(2, 0);
x0[0] = 12; x0[1] = 0.0;
vnl_matrix<double> P0(2, 2, 0);
P0(0, 0) = 0.1; P0(1, 1) = 0.1;
vnl_matrix<double> R(2, 2, 0);
R(0, 0) = 0.06; R(1, 1) = 0.01;
vnl_matrix<double> Q(2, 2, 0);
Q(0, 0) = 0.00000004; Q(1, 1) = 0.00000001;
cameraPlaningPan_EKF aEKF;
aEKF.init(x0, P0);
aEKF.initNoiseCovariance(Q, R);
for (int i = 0; i<observed_pan.size(); i++) {
vnl_vector<double> zk(2, 0);
zk[0] = observed_pan[i];
zk[1] = observed_velocity[i];
vnl_vector<double> xk;
vnl_matrix<double> pk;
aEKF.update(zk, xk, pk);
smoothed_pan.push_back(xk[0]);
smoothed_velocity.push_back(xk[1]);
}
vnl_vector<double> smoothedPanVec(&smoothed_pan[0], (int)smoothed_pan.size());
vnl_vector<double> smoothedVelocityVec(&smoothed_velocity[0], (int)smoothed_velocity.size());
vcl_string save_file("EKF_observed_pan_velocity_26.mat");
vnl_matlab_filewrite awriter(save_file.c_str());
awriter.write(smoothedPanVec, "sm_pan");
awriter.write(panMat.get_column(1), "pan");
awriter.write(panGdMat.get_column(1), "gdPan");
awriter.write(velocityMat.get_column(1), "velo");
awriter.write(smoothedVelocityVec, "sm_velo");
printf("save to %s\n", save_file.c_str());
}
// eq 18
double P0M(double a, double LE)
{
return P0(LE) + a * pow(sin(2 * t12), 2) * cos(2 * t12) * pow(cos(t13), 4) * (1.267 * LE * sin(2 * F21(LE)) - 2 * pow(sin(F21(LE)), 2) / Dmsq21);
}
EpipolaireCoordinate * EpipolaireCoordinateNoDist::MapingChScale(REAL aChSacle) const
{
EpipolaireCoordinateNoDist * aRes = new EpipolaireCoordinateNoDist(P0(),DirX());
aRes->HeriteChScale(const_cast<EpipolaireCoordinateNoDist&>(*this),aChSacle);
return aRes;
}
void sm3_compress(uint32_t digest[8], const unsigned char block[64])
{
int j;
uint32_t W[68], W1[64];
const uint32_t *pblock = (const uint32_t *)block;
uint32_t A = digest[0];
uint32_t B = digest[1];
uint32_t C = digest[2];
uint32_t D = digest[3];
uint32_t E = digest[4];
uint32_t F = digest[5];
uint32_t G = digest[6];
uint32_t H = digest[7];
uint32_t SS1,SS2,TT1,TT2,T[64];
for (j = 0; j < 16; j++) {
W[j] = cpu_to_be32(pblock[j]);
}
for (j = 16; j < 68; j++) {
W[j] = P1( W[j-16] ^ W[j-9] ^ ROTATELEFT(W[j-3],15)) ^ ROTATELEFT(W[j - 13],7 ) ^ W[j-6];;
}
for( j = 0; j < 64; j++) {
W1[j] = W[j] ^ W[j+4];
}
for(j =0; j < 16; j++) {
T[j] = 0x79CC4519;
SS1 = ROTATELEFT((ROTATELEFT(A,12) + E + ROTATELEFT(T[j],j)), 7);
SS2 = SS1 ^ ROTATELEFT(A,12);
TT1 = FF0(A,B,C) + D + SS2 + W1[j];
TT2 = GG0(E,F,G) + H + SS1 + W[j];
D = C;
C = ROTATELEFT(B,9);
B = A;
A = TT1;
H = G;
G = ROTATELEFT(F,19);
F = E;
E = P0(TT2);
}
for(j =16; j < 64; j++) {
T[j] = 0x7A879D8A;
SS1 = ROTATELEFT((ROTATELEFT(A,12) + E + ROTATELEFT(T[j],j)), 7);
SS2 = SS1 ^ ROTATELEFT(A,12);
TT1 = FF1(A,B,C) + D + SS2 + W1[j];
TT2 = GG1(E,F,G) + H + SS1 + W[j];
D = C;
C = ROTATELEFT(B,9);
B = A;
A = TT1;
H = G;
G = ROTATELEFT(F,19);
F = E;
E = P0(TT2);
}
digest[0] ^= A;
digest[1] ^= B;
digest[2] ^= C;
digest[3] ^= D;
digest[4] ^= E;
digest[5] ^= F;
digest[6] ^= G;
digest[7] ^= H;
}
void MagneticBubbleBoundary::set_bc_z0_wall(std::valarray<double> &U) const
{
const int Nx = Mara->domain->get_N(1);
const int Ny = Mara->domain->get_N(2);
const int Ng = Mara->domain->get_Ng();
const double r0 = rotation_radius;
ValarrayManager M(Mara->domain->aug_shape(), Mara->domain->get_Nq());
for (int i=0; i<Nx+2*Ng; ++i) {
for (int j=0; j<Ny+2*Ng; ++j) {
for (int k=0; k<Ng; ++k) {
double x = Mara->domain->x_at(i);
double y = Mara->domain->y_at(j);
double r = sqrt(x*x + y*y);
double Omega;
switch (rotation_profile) {
case RIGID_ROTATION:
if (r < r0) {
Omega = 1.0;
}
else {
Omega = 0.0;
}
break;
case RIGID_IN_KEPLARIAN_OUT:
if (r < 0.5*r0) {
Omega = 1.0;
}
else if (r > 2*r0) {
Omega = 0.0;
}
else {
Omega = pow(2*r/r0, -1.5);
}
break;
}
std::valarray<double> U1 = U[ M(i,j,2*Ng-k-1) ]; // reflected zone
std::valarray<double> P1(8); // reflected zone primitive variables
std::valarray<double> U0(8);
std::valarray<double> P0(8);
if (receive_primitive) {
P1 = U1; // U was actually P (confusing, I know)
}
else {
int err = Mara->fluid->ConsToPrim(&U1[0], &P1[0]);
if (err) {
/*
fprintf(stderr,
"[MagneticBubbleBoundary] unphysical boundary value\n");
std::cerr << Mara->fluid->PrintCons(&U1[0]) << std::endl;
std::cerr << rmhd_c2p_get_error(err) << std::endl;
*/
// exit(1);
continue;
}
}
/* not reflecting vz ensures that v < 1 in the guard zones */
P0[0] = P1[0];
P0[1] = P1[1];
P0[2] = -y/r0 * Omega;
P0[3] = x/r0 * Omega;
P0[4] = 0.0;//-P1[4];
P0[5] = -P1[5];
P0[6] = -P1[6];
P0[7] = P1[7];
if (receive_primitive) {
U0 = P0; // U really is P
int err = rmhd_c2p_check_prim(&P0[0]);
if (err) {
std::cerr << rmhd_c2p_get_error(err) << std::endl;
exit(1);
}
}
else {
int err = Mara->fluid->PrimToCons(&P0[0], &U0[0]);
if (err) {
std::cerr << rmhd_c2p_get_error(err) << std::endl;
exit(1);
}
}
U[ M(i,j,k) ] = U0;
}
}
}
}
void
MAST::GCMMAOptimizationInterface::optimize() {
#if MAST_ENABLE_GCMMA == 1
// make sure that all processes have the same problem setup
_feval->sanitize_parallel();
int
N = _feval->n_vars(),
M = _feval->n_eq() + _feval->n_ineq(),
n_rel_change_iters = _feval->n_iters_relative_change();
libmesh_assert_greater(N, 0);
std::vector<Real> XVAL(N, 0.), XOLD1(N, 0.), XOLD2(N, 0.),
XMMA(N, 0.), XMIN(N, 0.), XMAX(N, 0.), XLOW(N, 0.), XUPP(N, 0.),
ALFA(N, 0.), BETA(N, 0.), DF0DX(N, 0.),
A(M, 0.), B(M, 0.), C(M, 0.), Y(M, 0.), RAA(M, 0.), ULAM(M, 0.),
FVAL(M, 0.), FAPP(M, 0.), FNEW(M, 0.), FMAX(M, 0.),
DFDX(M*N, 0.), P(M*N, 0.), Q(M*N, 0.), P0(N, 0.), Q0(N, 0.),
UU(M, 0.), GRADF(M, 0.), DSRCH(M, 0.), HESSF(M*(M+1)/2, 0.),
f0_iters(n_rel_change_iters);
std::vector<int> IYFREE(M, 0);
std::vector<bool> eval_grads(M, false);
Real
ALBEFA = 0.1,
GHINIT = 0.5,
GHDECR = 0.7,
GHINCR = 1.2,
F0VAL = 0.,
F0NEW = 0.,
F0APP = 0.,
RAA0 = 0.,
Z = 0.,
GEPS =_feval->tolerance();
/*C********+*********+*********+*********+*********+*********+*********+
C
C The meaning of some of the scalars and vectors in the program:
C
C N = Complex of variables x_j in the problem.
C M = Complex of constraints in the problem (not including
C the simple upper and lower bounds on the variables).
C ALBEFA = Relative spacing between asymptote and mode limit. Lower value
C will cause the move limit (alpha,beta) to move closer to asymptote
C values (l, u).
C GHINIT = Initial asymptote setting. For the first two iterations the
C asymptotes (l, u) are defined based on offsets from the design
C point as this fraction of the design variable bounds, ie.
C l_j = x_j^k - GHINIT * (x_j^max - x_j^min)
C u_j = x_j^k + GHINIT * (x_j^max - x_j^min)
C GHDECR = Fraction by which the asymptote is reduced for oscillating
C changes in design variables based on three consecutive iterations
C GHINCR = Fraction by which the asymptote is increased for non-oscillating
C changes in design variables based on three consecutive iterations
C INNMAX = Maximal number of inner iterations within each outer iter.
C A reasonable choice is INNMAX=10.
C ITER = Current outer iteration number ( =1 the first iteration).
C GEPS = Tolerance parameter for the constraints.
C (Used in the termination criteria for the subproblem.)
C
C XVAL(j) = Current value of the variable x_j.
C XOLD1(j) = Value of the variable x_j one iteration ago.
C XOLD2(j) = Value of the variable x_j two iterations ago.
C XMMA(j) = Optimal value of x_j in the MMA subproblem.
C XMIN(j) = Original lower bound for the variable x_j.
C XMAX(j) = Original upper bound for the variable x_j.
C XLOW(j) = Value of the lower asymptot l_j.
C XUPP(j) = Value of the upper asymptot u_j.
C ALFA(j) = Lower bound for x_j in the MMA subproblem.
C BETA(j) = Upper bound for x_j in the MMA subproblem.
C F0VAL = Value of the objective function f_0(x)
C FVAL(i) = Value of the i:th constraint function f_i(x).
C DF0DX(j) = Derivative of f_0(x) with respect to x_j.
C FMAX(i) = Right hand side of the i:th constraint.
C DFDX(k) = Derivative of f_i(x) with respect to x_j,
C where k = (j-1)*M + i.
C P(k) = Coefficient p_ij in the MMA subproblem, where
C k = (j-1)*M + i.
C Q(k) = Coefficient q_ij in the MMA subproblem, where
C k = (j-1)*M + i.
C P0(j) = Coefficient p_0j in the MMA subproblem.
C Q0(j) = Coefficient q_0j in the MMA subproblem.
C B(i) = Right hand side b_i in the MMA subproblem.
C F0APP = Value of the approximating objective function
C at the optimal soultion of the MMA subproblem.
C FAPP(i) = Value of the approximating i:th constraint function
C at the optimal soultion of the MMA subproblem.
C RAA0 = Parameter raa_0 in the MMA subproblem.
C RAA(i) = Parameter raa_i in the MMA subproblem.
C Y(i) = Value of the "artificial" variable y_i.
C Z = Value of the "minimax" variable z.
C A(i) = Coefficient a_i for the variable z.
C C(i) = Coefficient c_i for the variable y_i.
C ULAM(i) = Value of the dual variable lambda_i.
C GRADF(i) = Gradient component of the dual objective function.
C DSRCH(i) = Search direction component in the dual subproblem.
C HESSF(k) = Hessian matrix component of the dual function.
C IYFREE(i) = 0 for dual variables which are fixed to zero in
C the current subspace of the dual subproblem,
C = 1 for dual variables which are "free" in
C the current subspace of the dual subproblem.
C
C********+*********+*********+*********+*********+*********+*********+*/
/*
* The USER should now give values to the parameters
* M, N, GEPS, XVAL (starting point),
* XMIN, XMAX, FMAX, A and C.
*/
// _initi(M,N,GEPS,XVAL,XMIN,XMAX,FMAX,A,C);
// Assumed: FMAX == A
_feval->_init_dvar_wrapper(XVAL, XMIN, XMAX);
// set the value of C[i] to be very large numbers
Real max_x = 0.;
for (unsigned int i=0; i<N; i++)
if (max_x < fabs(XVAL[i]))
max_x = fabs(XVAL[i]);
std::fill(C.begin(), C.end(), std::max(1.e0*max_x, _constr_penalty));
int INNMAX=_max_inner_iters, ITER=0, ITE=0, INNER=0, ICONSE=0;
/*
* The outer iterative process starts.
*/
bool terminate = false, inner_terminate=false;
while (!terminate) {
ITER=ITER+1;
ITE=ITE+1;
/*
* The USER should now calculate function values and gradients
* at XVAL. The result should be put in F0VAL,DF0DX,FVAL,DFDX.
*/
std::fill(eval_grads.begin(), eval_grads.end(), true);
_feval->_evaluate_wrapper(XVAL,
F0VAL, true, DF0DX,
FVAL, eval_grads, DFDX);
if (ITER == 1)
// output the very first iteration
_feval->_output_wrapper(0, XVAL, F0VAL, FVAL, true);
/*
* RAA0,RAA,XLOW,XUPP,ALFA and BETA are calculated.
*/
raasta_(&M, &N, &RAA0, &RAA[0], &XMIN[0], &XMAX[0], &DF0DX[0], &DFDX[0]);
asympg_(&ITER, &M, &N, &ALBEFA, &GHINIT, &GHDECR, &GHINCR,
&XVAL[0], &XMIN[0], &XMAX[0], &XOLD1[0], &XOLD2[0],
&XLOW[0], &XUPP[0], &ALFA[0], &BETA[0]);
/*
* The inner iterative process starts.
*/
// write the asymptote data for the inneriterations
_output_iteration_data(ITER, XVAL, XMIN, XMAX, XLOW, XUPP, ALFA, BETA);
INNER=0;
inner_terminate = false;
while (!inner_terminate) {
/*
* The subproblem is generated and solved.
*/
mmasug_(&ITER, &M, &N, &GEPS, &IYFREE[0], &XVAL[0], &XMMA[0],
&XMIN[0], &XMAX[0], &XLOW[0], &XUPP[0], &ALFA[0], &BETA[0],
&A[0], &B[0], &C[0], &Y[0], &Z, &RAA0, &RAA[0], &ULAM[0],
&F0VAL, &FVAL[0], &F0APP, &FAPP[0], &FMAX[0], &DF0DX[0], &DFDX[0],
&P[0], &Q[0], &P0[0], &Q0[0], &UU[0], &GRADF[0], &DSRCH[0], &HESSF[0]);
/*
* The USER should now calculate function values at XMMA.
* The result should be put in F0NEW and FNEW.
*/
std::fill(eval_grads.begin(), eval_grads.end(), false);
_feval->_evaluate_wrapper(XMMA,
F0NEW, false, DF0DX,
FNEW, eval_grads, DFDX);
if (INNER >= INNMAX) {
libMesh::out
<< "** Max Inner Iter Reached: Terminating! Inner Iter = "
<< INNER << std::endl;
inner_terminate = true;
}
else {
/*
* It is checked if the approximations were conservative.
*/
conser_( &M, &ICONSE, &GEPS, &F0NEW, &F0APP, &FNEW[0], &FAPP[0]);
if (ICONSE == 1) {
libMesh::out
<< "** Conservative Solution: Terminating! Inner Iter = "
<< INNER << std::endl;
inner_terminate = true;
}
else {
/*
* The approximations were not conservative, so RAA0 and RAA
* are updated and one more inner iteration is started.
*/
INNER=INNER+1;
raaupd_( &M, &N, &GEPS, &XMMA[0], &XVAL[0],
&XMIN[0], &XMAX[0], &XLOW[0], &XUPP[0],
&F0NEW, &FNEW[0], &F0APP, &FAPP[0], &RAA0, &RAA[0]);
}
}
}
/*
* The inner iterative process has terminated, which means
* that an outer iteration has been completed.
* The variables are updated so that XVAL stands for the new
* outer iteration point. The fuction values are also updated.
*/
xupdat_( &N, &ITER, &XMMA[0], &XVAL[0], &XOLD1[0], &XOLD2[0]);
fupdat_( &M, &F0NEW, &FNEW[0], &F0VAL, &FVAL[0]);
/*
* The USER may now write the current solution.
*/
_feval->_output_wrapper(ITER, XVAL, F0VAL, FVAL, true);
f0_iters[(ITE-1)%n_rel_change_iters] = F0VAL;
/*
* One more outer iteration is started as long as
* ITE is less than MAXITE:
*/
if (ITE == _feval->max_iters()) {
libMesh::out
<< "GCMMA: Reached maximum iterations, terminating! "
<< std::endl;
terminate = true;
}
// relative change in objective
bool rel_change_conv = true;
Real f0_curr = f0_iters[n_rel_change_iters-1];
for (unsigned int i=0; i<n_rel_change_iters-1; i++) {
if (f0_curr > sqrt(GEPS))
rel_change_conv = (rel_change_conv &&
fabs(f0_iters[i]-f0_curr)/fabs(f0_curr) < GEPS);
else
rel_change_conv = (rel_change_conv &&
fabs(f0_iters[i]-f0_curr) < GEPS);
}
if (rel_change_conv) {
libMesh::out
<< "GCMMA: Converged relative change tolerance, terminating! "
<< std::endl;
terminate = true;
}
}
#endif //MAST_ENABLE_GCMMA == 1
}
int main(int argc, const char * argv[])
{
// UNIT JOB FOR CALIBRATION
// ======================
//
// ARG #1: JOB NUMBER
// ARG #2: NUMBER OF LHS EXECUTIONS PER JOB
// ARG #3: MONTE CARLO RUNS (FOR EACH LHS)
// For performance monitoring
// - do not delete -
timeval tim;
gettimeofday(&tim, NULL);
double t1=tim.tv_sec+(tim.tv_usec/1000000.0);
if (argc<3)
{
cout <<endl << " ERROR in Job Unit ('main_jobUnit.cpp'): not enough arguments" << endl;
exit(1);
}
cout << endl << " === JOB UNIT #"<<argv[1]<<" (nrows="<< argv[2] <<") ==="<<endl;
// Parameters file name
string fname = _DIR_CALIB + "calib_LHS_SF_input_" ;
int i_job = atoi(argv[1]);
int nrows = atoi(argv[2]);
int nMC_calibration = atoi(argv[3]);
// === PRE CALIBRATION ===
// Start from an unpartnered population
// Initialize empty Population object
Population P0(0);
// Read starting population from a file
// output of "generateIndividuals.R"
P0.initFromFile("startPopulation.csv",
"in_STI.csv",
"in_STI_SFincrease.csv",
"in_HIVrebound.csv");
// Set all parameters
P0.setAllParameters();
// Set-up pre-calibration simulation
double horizon_preCalib = 60.0;
double timestep_preCalib = 0.25;
Simulation S_preCalib(horizon_preCalib, timestep_preCalib, P0, 0);
// Pre-calibration run to form partnerships first
bool logIndivInfo = false;
bool doSex = false;
bool traceNetwork = false;
bool displayProgress = 0;
S_preCalib.runAllEvents_horizon(doSex,
logIndivInfo,
traceNetwork,
displayProgress);
// Seed STIs' initial prevalence
S_preCalib.STI_set_initial_prevalence("in_STI_initial_prevalence.csv");
coutline(80);
cout << " PRE CALIBRATION POPULATION:";
S_preCalib.get_population().displayInfo(false);
Population P1 = S_preCalib.get_population();
// == END PRE-CALIBRATION ==
// =================
// Horizon for the calibration
string file_calibration = _DIR_CALIB + "in_calibration.csv";
double horizon_calib = getParameterFromFile("horizon_calib", file_calibration);
double timeStep_calib = getParameterFromFile("timeStep_calib", file_calibration);
// Runs the calibration using the
// parameters limits defined in the files defined by the job number
calibration_LHS_fromFile(_DIR_CALIB + "calibration_filename_wrapper.csv",
// Parameters (pre-sampled)
fname + "DMG_"+int2string(i_job)+".csv",
fname + "FORM_"+int2string(i_job)+".csv",
fname + "SPOUSAL_"+int2string(i_job)+".csv",
fname + "DISSOL_"+int2string(i_job)+".csv",
fname + "SEXACT_"+int2string(i_job)+".csv",
fname + "STIFTR_V_"+int2string(i_job)+".csv",
fname + "STIFTR_B_"+int2string(i_job)+".csv",
fname + "STIFTR_P_"+int2string(i_job)+".csv",
fname + "STIFTR_F_"+int2string(i_job)+".csv",
nrows, i_job,
P1, horizon_calib, timeStep_calib,
nMC_calibration);
// --------------------------------------------------------------
// COMPUTER TIME MONITORING - do not delete!
gettimeofday(&tim, NULL);
double t2=tim.tv_sec+(tim.tv_usec/1000000.0);
int minutes = (int)((t2-t1)/60.0);
double sec = (t2-t1)-minutes*60.0;
// --------------------------------------------------------------
cout << endl << "~~~> JOB UNIT #"<<argv[1]<<" was completed in ";
cout << minutes<<" min "<<sec<<" sec" << endl;
return 0;
}
int estimate::compute ( double cog ,double *zmp ,double cogaccel,int estimatecount, int direction ) {
//direction = 1 ,lateral direction
//direction = 2 ,saggital direction
const unsigned NTRY = 1200*10; //data计
const unsigned n = 3; //nb states
const unsigned m = 3; //nb measures
double timestep = 0.005 ;
double Dzmp = 0 , Dzmpbefore = 0 ; //玡ㄨzmp Ω稬だ秖
double DDzmp = 0 , DDzmpbefore = 0 ; //玡ㄨzmp Ω稬だ秖
double IMU_COG = 666/732;
//static const double _P0[] = { 0.1 ,0.0, 0.0, 0.0,
// 0.0, 0.1 , 0.0 ,0.0,
// 0.0, 0.0, 0.1 ,0.0,
// 0.0 , 0.0 ,0.0 ,0.1
//
// }; //initial error
static const double _P0[] = { 0.1 ,0.0, 0.0,
0.0, 0.1 , 0.0 ,
0.0, 0.0, 0.1
}; //initial error
Vector x(n); // 秨state vector
Vector state(n) ;
Vector z(m);
Matrix P0(n, n, _P0); //ミ initial error matrix Τ脄奔20140120
//P0(0,0) = 0 ;
//zmp'' 计稬だ矪瞶
if( estimatecount < 2) {
Dzmp = 0 ;
Dzmpbefore = 0 ;
}
else{
Dzmp = ( *zmp - *(zmp-2) ) / timestep ;
Dzmpbefore = ( *(zmp-2) - *(zmp-4) ) / timestep ;
}
DDzmp = ( Dzmp - Dzmpbefore )/ timestep ;
//P0(0,0) = 0 ;
filter.init(x, P0);
z(1) = cog;
z(2) = *zmp;
z(3) = cogaccel* IMU_COG ; //盢measurement 糶 IMU杆竚ゑㄒだ皌
// u control input zmpΩ稬だ
Vector u(1);
u(1) = DDzmp ;
filter.step(u, z);
state = filter.getX() ;
if (direction == 1 ) {
Cogstatelateral.push_back (state (1)) ; //盢state cogstate matrix
Dcogstatelateral.push_back (state (2)) ;
zmpstatelateral.push_back (state (3)) ;
//cout << "xp(" << ":," << estimatecount<<") = " <<setprecision(3)<< Cogstatelateral[estimatecount]<<" "<<setprecision(3)<<Dcogstatelateral[estimatecount]<<" "<<setprecision(3)<< zmpstatelateral[estimatecount]<<" "<< setprecision(3)<< externalforcelateral[estimatecount] <<endl;
//setprecision(3) cout计翴计
}
else if (direction ==2){
Cogstatesaggital.push_back (state (1)) ; //盢state cogstate matrix
Dcogstatesaggital.push_back (state (2)) ;
zmpstatesaggital.push_back (state (3)) ;
//cout << "xp(" << ":," << estimatecount<<") = " <<setprecision(3)<< Cogstatesaggital[estimatecount]<<" "<<setprecision(3)<<Dcogstatesaggital[estimatecount]<<" "<<setprecision(3)<< zmpstatesaggital[estimatecount]<<" "<< setprecision(3)<< externalforcesaggital[estimatecount] <<endl;
}
return EXIT_SUCCESS;
}
void run_stresstest_2proc()
{
std::cout << "Running cache hierarchy correctness test..." << std::endl;
Addr addr_space = 4*GB;
size_t mem_access_cost = 500;
size_t L2_direct_entries = 512;
size_t L2_line_size_bytes = 128;
size_t L2_associativity = 8;
size_t L2_hit_cost_ticks = 50;
size_t L1_direct_entries = 128;
size_t L1_line_size_bytes = 64;
size_t L1_associativity = 2;
size_t L1_hit_cost_ticks = 3;
MainMemory main_mem(addr_space, mem_access_cost);
Cache L2("L2", &main_mem, L2_direct_entries, L2_associativity, L2_line_size_bytes, L2_hit_cost_ticks, IS_WRITEBACK_CACHE);
Cache L1P0("L1P0", &L2, L1_direct_entries, L1_associativity, L1_line_size_bytes, L1_hit_cost_ticks, !IS_WRITEBACK_CACHE);
Cache L1P1("L1P1", &L2, L1_direct_entries, L1_associativity, L1_line_size_bytes, L1_hit_cost_ticks, !IS_WRITEBACK_CACHE);
Processor P0("P0", &L1P0);
Processor P1("P1", &L1P1);
for(size_t i=0; i<globalmem_size; i++) {
globalmem[i]=0;
}
#ifdef WINTIME
long int before = GetTickCount();
#else
clock_t start, finish;
start = clock();
#endif
size_t iterations = 100*K;
size_t ticksP0 = 0, ticksP1 = 0;
for (size_t i=0; i<iterations; i++)
{
//int control_sum=0;
//for(size_t i=0; i<globalmem_size; i++) {
// control_sum += globalmem[i];
//}
//assert(control_sum==0);
Addr addr1P0 = (Addr)&globalmem[rand()%globalmem_size];
Addr addr2P0 = (Addr)&globalmem[rand()%globalmem_size];
Addr addr1P1 = (Addr)&globalmem[rand()%globalmem_size];
Addr addr2P1 = (Addr)&globalmem[rand()%globalmem_size];
int vals[4] = {0, 0, 0, 0};
P0.read(addr1P0, vals[0], ticksP0);
P0.write(addr1P0, ++vals[0], ticksP0);
P1.read(addr1P1, vals[1], ticksP1);
P1.write(addr1P1, ++vals[1], ticksP1);
P0.read(addr2P0, vals[2], ticksP0);
P0.write(addr2P0, --vals[2], ticksP0);
P1.read(addr2P1, vals[3], ticksP1);
P1.write(addr2P1, --vals[3], ticksP1);
//main_mem.reset();
//for(size_t i=0; i<globalmem_size; i++) {
// control_sum += globalmem[i];
//}
//if (control_sum!=0) {
//P0.dump(std::cout);
//P1.dump(std::cout);
//}
//assert(control_sum==0);
//std::cout << std::endl;
};
#ifdef WINTIME
long int after = GetTickCount();
std::cout << "Execution Time: " << (after-before) << " ms." << std::endl;
#else
finish = clock();
double exec_time_ms = ((double)(finish - start))*1000/CLOCKS_PER_SEC;
std::cout << "Execution Time: " << exec_time_ms << " ms." << std::endl;
std::cout << "that makes: " << ((double)(iterations/exec_time_ms)) << " cache queries/ms." << std::endl;
#endif
main_mem.reset();
int control_sum=0;
for(size_t i=0; i<globalmem_size; i++) {
control_sum += globalmem[i];
}
std::cout << "Control sum: " << control_sum << " (should be 0)" << std::endl;
}
bool Rectangle::Intersect(const Ray &r, float *tHit, float *rayEpsilon,
DifferentialGeometry *dg) const {
// Transform _Ray_ to object space
Ray ray;
(*WorldToObject)(r, &ray);
// Compute plane intersection for disk
// Checks if the plane is parallel to the ray or not
// We can get the direction of the ray
// If the Z component of the direction of the ray is zero
// then, the ray is parallel to the plane and in such case
// there is no intersection point between the ray and the plane.
if (fabsf(ray.d.z) < 1e-7)
return false;
// Now, the direction of the ray is not parallel to the plane
// We have to check if the intersection happens or not
// We have to compute the parametric t where the ray intersects the plane
// We want to find t such that the z-component of the ray intersects the plane
// The ray "line" equation is l = l0 + (l1 - l0) * t
// l1 - l0 will give us the distance between the two points on the plane
// Then t is the ratio and in such case it should be between 0 and 1
// Considering that the rectangle completely lies in the z plane
/// distance = l1 - l0
/// thit = (l - l0) / distance
// But since we assume that the plane is located at height
// Then, the point l is at height on the plane
/// l = height
float thit = (height - ray.o.z) / ray.d.z;
// Then we check if the thit is between the ratio of 0 and 1 that is mapped
// between ray.mint and ray.maxt, if not retrun false
if (thit < ray.mint || thit > ray.maxt)
return false;
// Then we see if the point lies inside the disk or not
// Substitute the thit in the ray equation to get hit point on the ray
Point phit = ray(thit);
// We have to make sure that the interesction lies inside the plane
if (!(phit.x < x/2 && phit.x > -x/2 && phit.y < y/2 && phit.y > -y/2))
return false;
// Assuming that the plane is formed from the following 4 points
// P0, P1, P2, P3
//
// p0 *---------------* p1
// | |
// | |
// | |
// | O |
// | |
// | |
// | |
// p2 *---------------* p3 -> X
//
// P0 @ (-x/2, y/2)
// P1 @ (x/2, y/2)
// P2 @ (-x/2, -y/2)
// P3 @ (x/2, -y/2)
Point P0(-x/2, y/2, height), P1(x/2, y/2, height);
Point P2(-x/2, -y/2, height), P3(x/2, -y/2, height);
/// Now, we have to find the parametric form of the plane in terms of (u,v)
/// Plane equation can be formed by at least 3 points P0, P1, P2
/// P0 -> P1 (vector 1)
/// P0 -> p2 (vector 2)
/// An arbitrary point on the plane p is found in the following parametric form
/// P = P0 + (P1 - P0) u + (P2 - P0) v
/// Now we need to express two explicit equations of u and v
/// So, we have to construct the system of equation that solves for u and v
///
/// Since we have found the intersection point between the plane and the line
/// we have to use it to formalize the system of equations that will be used
/// to find the parametric form of the plane
/// Plane equation is : P = P0 + (P1 - P0) u + (P2 - P0) v
/// Ray equation is : l = l0 + (l1 - l0) * thit
/// But l = P, then
/// l0 + (l1 - l0) * thit = P0 + (P1 - P0) * u + (P2 - P0) * v
/// l0 - P0 = (l0 - l1) * thit + (P1 - P0) * u + (P2 - P0) * v
/// MAPPING : l0 = ray.o
/// [l0.x - P0.x] = [l0.x - l1.x P1.x - P0.x P2.x - P0.x] [t]
/// [l0.y - P0.y] = [l0.y - l1.y P1.y - P0.y P2.y - P0.y] [u]
/// [l0.z - P0.z] = [l0.z - l1.z P1.z - P0.z P2.z - P0.z] [v]
///
/// Then, we should find the inverse of the matrix in order to
/// solve for u,v and t for check
// System AX = B
float a11 = ray.o.x - 0;
float a12 = P1.x - P0.x;
float a13 = P2.x - P0.x;
float a21 = ray.o.y - 0;
float a22 = P1.y - P0.y;
float a23 = P2.y - P0.y;
float a31 = ray.o.y - height;
float a32 = P1.z - P0.z;
float a33 = P2.z - P0.z;
float b1 = -7;
float b2 = -2;
float b3 = 14;
float x1 = 0;
float x2 = 0;
float x3 = 0;
Imath::M33f A(a11,a12,a13,a21,a22,a23,a31,a32,a33), AInverted;
Imath::V3f X(x1, x2, x3);
Imath::V3f B(b1,b2, b3);
// This operation has been checked and working for getting
// the correct inverse of the matrix A
AInverted = A.invert(false);
x1 = AInverted[0][0] * B[0] +
AInverted[0][1] * B[1] +
AInverted[0][2] * B[2];
x2 = AInverted[1][0] * B[0] +
AInverted[1][1] * B[1] +
AInverted[1][2] * B[2];
x3 = AInverted[2][0] * B[0] +
AInverted[2][1] * B[1] +
AInverted[2][2] * B[2];
/// Then we have u = something, and v = something
///
/// Then we come for the derivatives, so we have to find the derivatives
/// from the parametric forms defined above for the plane equations
/// dpdu = (P1 - P0)
/// dpdv = (P2 - P0)
///
/// For the normal we have the always fixed direction in y
/// So the derivative for the normal is zero
/// dndu = (0, 0, 0) dndv = (0, 0, 0)
///
/// Then we can construct the DifferentilGeometry and go ahead
// Find parametric representation of disk hit
float u = x2;
float v = x3;
Vector dpdu(P1.x - P0.x, P1.y - P0.y, P1.z - P0.z);
Vector dpdv(P2.x - P0.x, P2.y - P0.y, P2.z - P0.z);
Normal dndu(0,0,0), dndv(0,0,0);
// Initialize _DifferentialGeometry_ from parametric information
const Transform &o2w = *ObjectToWorld;
*dg = DifferentialGeometry(o2w(phit), o2w(dpdu), o2w(dpdv),
o2w(dndu), o2w(dndv), u, v, this);
// Update _tHit_ for quadric intersection
*tHit = thit;
// Compute _rayEpsilon_ for quadric intersection
*rayEpsilon = 5e-4f * *tHit;
return true;
}
/*
* SM3 Compression Function
*/
void SM3::compress_n(const uint8_t input[], size_t blocks)
{
uint32_t A = m_digest[0], B = m_digest[1], C = m_digest[2], D = m_digest[3],
E = m_digest[4], F = m_digest[5], G = m_digest[6], H = m_digest[7];
uint32_t W[68], W1[64];
uint32_t SS1, SS2, TT1, TT2, T[64];
for(size_t i = 0; i != blocks; ++i)
{
// Message Extension (a)
W[ 0] = load_be<uint32_t>(input, 0);
W[ 1] = load_be<uint32_t>(input, 1);
W[ 2] = load_be<uint32_t>(input, 2);
W[ 3] = load_be<uint32_t>(input, 3);
W[ 4] = load_be<uint32_t>(input, 4);
W[ 5] = load_be<uint32_t>(input, 5);
W[ 6] = load_be<uint32_t>(input, 6);
W[ 7] = load_be<uint32_t>(input, 7);
W[ 8] = load_be<uint32_t>(input, 8);
W[ 9] = load_be<uint32_t>(input, 9);
W[10] = load_be<uint32_t>(input, 10);
W[11] = load_be<uint32_t>(input, 11);
W[12] = load_be<uint32_t>(input, 12);
W[13] = load_be<uint32_t>(input, 13);
W[14] = load_be<uint32_t>(input, 14);
W[15] = load_be<uint32_t>(input, 15);
// Message Extension (b)
W[16] = P1(W[ 0] ^ W[ 7] ^ rotate_left(W[13], 15)) ^ rotate_left(W[ 3], 7) ^ W[10];
W[17] = P1(W[ 1] ^ W[ 8] ^ rotate_left(W[14], 15)) ^ rotate_left(W[ 4], 7) ^ W[11];
W[18] = P1(W[ 2] ^ W[ 9] ^ rotate_left(W[15], 15)) ^ rotate_left(W[ 5], 7) ^ W[12];
W[19] = P1(W[ 3] ^ W[10] ^ rotate_left(W[16], 15)) ^ rotate_left(W[ 6], 7) ^ W[13];
W[20] = P1(W[ 4] ^ W[11] ^ rotate_left(W[17], 15)) ^ rotate_left(W[ 7], 7) ^ W[14];
W[21] = P1(W[ 5] ^ W[12] ^ rotate_left(W[18], 15)) ^ rotate_left(W[ 8], 7) ^ W[15];
W[22] = P1(W[ 6] ^ W[13] ^ rotate_left(W[19], 15)) ^ rotate_left(W[ 9], 7) ^ W[16];
W[23] = P1(W[ 7] ^ W[14] ^ rotate_left(W[20], 15)) ^ rotate_left(W[10], 7) ^ W[17];
W[24] = P1(W[ 8] ^ W[15] ^ rotate_left(W[21], 15)) ^ rotate_left(W[11], 7) ^ W[18];
W[25] = P1(W[ 9] ^ W[16] ^ rotate_left(W[22], 15)) ^ rotate_left(W[12], 7) ^ W[19];
W[26] = P1(W[10] ^ W[17] ^ rotate_left(W[23], 15)) ^ rotate_left(W[13], 7) ^ W[20];
W[27] = P1(W[11] ^ W[18] ^ rotate_left(W[24], 15)) ^ rotate_left(W[14], 7) ^ W[21];
W[28] = P1(W[12] ^ W[19] ^ rotate_left(W[25], 15)) ^ rotate_left(W[15], 7) ^ W[22];
W[29] = P1(W[13] ^ W[20] ^ rotate_left(W[26], 15)) ^ rotate_left(W[16], 7) ^ W[23];
W[30] = P1(W[14] ^ W[21] ^ rotate_left(W[27], 15)) ^ rotate_left(W[17], 7) ^ W[24];
W[31] = P1(W[15] ^ W[22] ^ rotate_left(W[28], 15)) ^ rotate_left(W[18], 7) ^ W[25];
W[32] = P1(W[16] ^ W[23] ^ rotate_left(W[29], 15)) ^ rotate_left(W[19], 7) ^ W[26];
W[33] = P1(W[17] ^ W[24] ^ rotate_left(W[30], 15)) ^ rotate_left(W[20], 7) ^ W[27];
W[34] = P1(W[18] ^ W[25] ^ rotate_left(W[31], 15)) ^ rotate_left(W[21], 7) ^ W[28];
W[35] = P1(W[19] ^ W[26] ^ rotate_left(W[32], 15)) ^ rotate_left(W[22], 7) ^ W[29];
W[36] = P1(W[20] ^ W[27] ^ rotate_left(W[33], 15)) ^ rotate_left(W[23], 7) ^ W[30];
W[37] = P1(W[21] ^ W[28] ^ rotate_left(W[34], 15)) ^ rotate_left(W[24], 7) ^ W[31];
W[38] = P1(W[22] ^ W[29] ^ rotate_left(W[35], 15)) ^ rotate_left(W[25], 7) ^ W[32];
W[39] = P1(W[23] ^ W[30] ^ rotate_left(W[36], 15)) ^ rotate_left(W[26], 7) ^ W[33];
W[40] = P1(W[24] ^ W[31] ^ rotate_left(W[37], 15)) ^ rotate_left(W[27], 7) ^ W[34];
W[41] = P1(W[25] ^ W[32] ^ rotate_left(W[38], 15)) ^ rotate_left(W[28], 7) ^ W[35];
W[42] = P1(W[26] ^ W[33] ^ rotate_left(W[39], 15)) ^ rotate_left(W[29], 7) ^ W[36];
W[43] = P1(W[27] ^ W[34] ^ rotate_left(W[40], 15)) ^ rotate_left(W[30], 7) ^ W[37];
W[44] = P1(W[28] ^ W[35] ^ rotate_left(W[41], 15)) ^ rotate_left(W[31], 7) ^ W[38];
W[45] = P1(W[29] ^ W[36] ^ rotate_left(W[42], 15)) ^ rotate_left(W[32], 7) ^ W[39];
W[46] = P1(W[30] ^ W[37] ^ rotate_left(W[43], 15)) ^ rotate_left(W[33], 7) ^ W[40];
W[47] = P1(W[31] ^ W[38] ^ rotate_left(W[44], 15)) ^ rotate_left(W[34], 7) ^ W[41];
W[48] = P1(W[32] ^ W[39] ^ rotate_left(W[45], 15)) ^ rotate_left(W[35], 7) ^ W[42];
W[49] = P1(W[33] ^ W[40] ^ rotate_left(W[46], 15)) ^ rotate_left(W[36], 7) ^ W[43];
W[50] = P1(W[34] ^ W[41] ^ rotate_left(W[47], 15)) ^ rotate_left(W[37], 7) ^ W[44];
W[51] = P1(W[35] ^ W[42] ^ rotate_left(W[48], 15)) ^ rotate_left(W[38], 7) ^ W[45];
W[52] = P1(W[36] ^ W[43] ^ rotate_left(W[49], 15)) ^ rotate_left(W[39], 7) ^ W[46];
W[53] = P1(W[37] ^ W[44] ^ rotate_left(W[50], 15)) ^ rotate_left(W[40], 7) ^ W[47];
W[54] = P1(W[38] ^ W[45] ^ rotate_left(W[51], 15)) ^ rotate_left(W[41], 7) ^ W[48];
W[55] = P1(W[39] ^ W[46] ^ rotate_left(W[52], 15)) ^ rotate_left(W[42], 7) ^ W[49];
W[56] = P1(W[40] ^ W[47] ^ rotate_left(W[53], 15)) ^ rotate_left(W[43], 7) ^ W[50];
W[57] = P1(W[41] ^ W[48] ^ rotate_left(W[54], 15)) ^ rotate_left(W[44], 7) ^ W[51];
W[58] = P1(W[42] ^ W[49] ^ rotate_left(W[55], 15)) ^ rotate_left(W[45], 7) ^ W[52];
W[59] = P1(W[43] ^ W[50] ^ rotate_left(W[56], 15)) ^ rotate_left(W[46], 7) ^ W[53];
W[60] = P1(W[44] ^ W[51] ^ rotate_left(W[57], 15)) ^ rotate_left(W[47], 7) ^ W[54];
W[61] = P1(W[45] ^ W[52] ^ rotate_left(W[58], 15)) ^ rotate_left(W[48], 7) ^ W[55];
W[62] = P1(W[46] ^ W[53] ^ rotate_left(W[59], 15)) ^ rotate_left(W[49], 7) ^ W[56];
W[63] = P1(W[47] ^ W[54] ^ rotate_left(W[60], 15)) ^ rotate_left(W[50], 7) ^ W[57];
W[64] = P1(W[48] ^ W[55] ^ rotate_left(W[61], 15)) ^ rotate_left(W[51], 7) ^ W[58];
W[65] = P1(W[49] ^ W[56] ^ rotate_left(W[62], 15)) ^ rotate_left(W[52], 7) ^ W[59];
W[66] = P1(W[50] ^ W[57] ^ rotate_left(W[63], 15)) ^ rotate_left(W[53], 7) ^ W[60];
W[67] = P1(W[51] ^ W[58] ^ rotate_left(W[64], 15)) ^ rotate_left(W[54], 7) ^ W[61];
// Message Extension (c)
W1[ 0] = W[ 0] ^ W[ 4];
W1[ 1] = W[ 1] ^ W[ 5];
W1[ 2] = W[ 2] ^ W[ 6];
W1[ 3] = W[ 3] ^ W[ 7];
W1[ 4] = W[ 4] ^ W[ 8];
W1[ 5] = W[ 5] ^ W[ 9];
W1[ 6] = W[ 6] ^ W[10];
W1[ 7] = W[ 7] ^ W[11];
W1[ 8] = W[ 8] ^ W[12];
W1[ 9] = W[ 9] ^ W[13];
W1[10] = W[10] ^ W[14];
W1[11] = W[11] ^ W[15];
W1[12] = W[12] ^ W[16];
W1[13] = W[13] ^ W[17];
W1[14] = W[14] ^ W[18];
W1[15] = W[15] ^ W[19];
W1[16] = W[16] ^ W[20];
W1[17] = W[17] ^ W[21];
W1[18] = W[18] ^ W[22];
W1[19] = W[19] ^ W[23];
W1[20] = W[20] ^ W[24];
W1[21] = W[21] ^ W[25];
W1[22] = W[22] ^ W[26];
W1[23] = W[23] ^ W[27];
W1[24] = W[24] ^ W[28];
W1[25] = W[25] ^ W[29];
W1[26] = W[26] ^ W[30];
W1[27] = W[27] ^ W[31];
W1[28] = W[28] ^ W[32];
W1[29] = W[29] ^ W[33];
W1[30] = W[30] ^ W[34];
W1[31] = W[31] ^ W[35];
W1[32] = W[32] ^ W[36];
W1[33] = W[33] ^ W[37];
W1[34] = W[34] ^ W[38];
W1[35] = W[35] ^ W[39];
W1[36] = W[36] ^ W[40];
W1[37] = W[37] ^ W[41];
W1[38] = W[38] ^ W[42];
W1[39] = W[39] ^ W[43];
W1[40] = W[40] ^ W[44];
W1[41] = W[41] ^ W[45];
W1[42] = W[42] ^ W[46];
W1[43] = W[43] ^ W[47];
W1[44] = W[44] ^ W[48];
W1[45] = W[45] ^ W[49];
W1[46] = W[46] ^ W[50];
W1[47] = W[47] ^ W[51];
W1[48] = W[48] ^ W[52];
W1[49] = W[49] ^ W[53];
W1[50] = W[50] ^ W[54];
W1[51] = W[51] ^ W[55];
W1[52] = W[52] ^ W[56];
W1[53] = W[53] ^ W[57];
W1[54] = W[54] ^ W[58];
W1[55] = W[55] ^ W[59];
W1[56] = W[56] ^ W[60];
W1[57] = W[57] ^ W[61];
W1[58] = W[58] ^ W[62];
W1[59] = W[59] ^ W[63];
W1[60] = W[60] ^ W[64];
W1[61] = W[61] ^ W[65];
W1[62] = W[62] ^ W[66];
W1[63] = W[63] ^ W[67];
for (size_t j = 0; j < 16; j++)
{
T[j] = SM3_TJ_0_15;
SS1 = rotate_left(rotate_left(A, 12) + E + rotate_left(T[j], j), 7);
SS2 = SS1 ^ rotate_left(A, 12);
TT1 = FF0(A, B, C) + D + SS2 + W1[j];
TT2 = GG0(E, F, G) + H + SS1 + W[j];
D = C;
C = rotate_left(B, 9);
B = A;
A = TT1;
H = G;
G = rotate_left(F, 19);
F = E;
E = P0(TT2);
}
for (size_t j = 16; j < 64; j++)
{
T[j] = SM3_TJ_16_63;
SS1 = rotate_left(rotate_left(A, 12) + E + rotate_left(T[j], j), 7);
SS2 = SS1 ^ rotate_left(A, 12);
TT1 = FF1(A, B, C) + D + SS2 + W1[j];
TT2 = GG1(E, F, G) + H + SS1 + W[j];
D = C;
C = rotate_left(B, 9);
B = A;
A = TT1;
H = G;
G = rotate_left(F, 19);
F = E;
E = P0(TT2);
}
A = (m_digest[0] ^= A);
B = (m_digest[1] ^= B);
C = (m_digest[2] ^= C);
D = (m_digest[3] ^= D);
E = (m_digest[4] ^= E);
F = (m_digest[5] ^= F);
G = (m_digest[6] ^= G);
H = (m_digest[7] ^= H);
input += hash_block_size();
}
}
void extr(jvec &ext_EP,jvec &ext_ED,jvec &ext_Q2,jvec &ext_fP,jvec &ext_fM,jvec &ext_f0,jvec &ext_fT,int il_sea,int il,int ic)
{
////////////////////////////////////////// R0 //////////////////////////////////////
jvec R0_corr;
jack R0(njack);
//load standing
jvec ll0_st=load_3pts("V0",il,il,0,RE,ODD,1);
jvec lc0_st=load_3pts("V0",ic,il,0,RE,ODD,1);
jvec cc0_st=load_3pts("V0",ic,ic,0,RE,ODD,1);
//build R0
R0_corr=lc0_st*lc0_st.simmetric()/(cc0_st*ll0_st);
//fit and plot
R0=constant_fit(R0_corr,TH-tmax,tmax,combine("plots/R0_il_%d_ic_%d.xmg",il,ic).c_str());
//////////////////////////////////////////// R2 ////////////////////////////////////
jvec R2_corr[nth];
jvec RT_corr[nth];
jvec R2(nth,njack);
jvec RT(nth,njack);
ofstream out_R2(combine("plots/R2_il_%d_ic_%d.xmg",il,ic).c_str());
ofstream out_RT(combine("plots/RT_il_%d_ic_%d.xmg",il,ic).c_str());
jvec lcK_th[nth],lc0_th[nth],lcT_th[nth];
for(int ith=0;ith<nth;ith++)
{
//load corrs
lcK_th[ith]=load_3pts("VK",ic,il,ith,IM,EVN,-1)/(6*th_P[ith]);
lc0_th[ith]=load_3pts("V0",ic,il,ith,RE,ODD,1);
lcT_th[ith]=load_3pts("VTK",ic,il,ith,IM,ODD,1)/(6*th_P[ith]);
//build ratios
R2_corr[ith]=lcK_th[ith]/lc0_th[ith];
RT_corr[ith]=lcT_th[ith]/lcK_th[ith];
//fit
R2[ith]=constant_fit(R2_corr[ith],tmin,tmax);
RT[ith]=constant_fit(RT_corr[ith],tmin,tmax);
//plot
out_R2<<write_constant_fit_plot(R2_corr[ith],R2[ith],tmin,tmax);
out_RT<<write_constant_fit_plot(RT_corr[ith],RT[ith],tmin,tmax);
}
////////////////////////////////////////// R1 //////////////////////////////////////
jvec R1_corr[nth];
jvec R1(nth,njack);
ofstream out_P(combine("plots/out_P_il_%d_ic_%d.xmg",il,ic).c_str());
out_P<<"@type xydy"<<endl;
ofstream out_D(combine("plots/out_D_il_%d_ic_%d.xmg",il,ic).c_str());
out_D<<"@type xydy"<<endl;
ofstream out_R1(combine("plots/out_R1_il_%d_ic_%d.xmg",il,ic).c_str());
out_R1<<"@type xydy"<<endl;
//load Pi and D
jvec P_corr[nth],D_corr[nth];
jvec ED(nth,njack),EP(nth,njack);
for(int ith=0;ith<nth;ith++)
{
//load moving pion
P_corr[ith]=load_2pts("2pts_P5P5.dat",il_sea,il,ith);
out_P<<"@type xydy"<<endl;
EP[ith]=constant_fit(effective_mass(P_corr[ith]),tmin_P,TH,combine("plots/P_eff_mass_il_%d_ic_%d_ith_%d.xmg",
il,ic,ith).c_str());
out_P<<write_constant_fit_plot(effective_mass(P_corr[ith]),EP[ith],tmin_P,TH);
out_P<<"&"<<endl;
//recompute EP and ED from standing one
if(ith)
{
ED[ith]=latt_en(ED[0],th_P[ith]);
EP[ith]=latt_en(EP[0],th_P[ith]);
}
//load moving D
D_corr[ith]=load_2pts("2pts_P5P5.dat",il,ic,ith);
out_D<<"@type xydy"<<endl;
ED[ith]=constant_fit(effective_mass(D_corr[ith]),tmin_D,TH,combine("plots/D_eff_mass_il_%d_ic_%d_ith_%d.xmg",
il,ic,ith).c_str());
out_D<<write_constant_fit_plot(effective_mass(D_corr[ith]),ED[ith],tmin_D,TH);
out_D<<"&"<<endl;
//build the ratio
R1_corr[ith]=lc0_th[ith]/lc0_th[0];
for(int t=0;t<TH;t++)
{
int E_fit_reco_flag=1;
jack Dt(njack),Pt(njack);
if(E_fit_reco_flag==0)
{
Dt=D_corr[0][t]/D_corr[ith][t];
Pt=P_corr[0][TH-t]/P_corr[ith][TH-t];
}
else
{
jack ED_th=latt_en(ED[0],th_P[ith]),EP_th=latt_en(EP[0],th_P[ith]);
Dt=exp(-(ED[0]-ED_th)*t)*ED_th/ED[0];
Pt=exp(-(EP[0]-EP_th)*(TH-t))*EP_th/EP[0];
}
R1_corr[ith][t]*=Dt*Pt;
}
//fit
R1[ith]=constant_fit(R1_corr[ith],tmin,tmax);
//plot
out_R1<<write_constant_fit_plot(R1_corr[ith],R1[ith],tmin,tmax);
}
//////////////////////////////////////// solve the ratios //////////////////////////////
//compute f0[q2max]
jvec f0_r(nth,njack),fP_r(nth,njack),fT_r(nth,njack);
f0_r[0]=sqrt(R0*4*ED[0]*EP[0])/(ED[0]+EP[0]);
cout<<"f0_r[q2max]: "<<f0_r[0]<<endl;
//compute QK and Q2
double mom[nth];
jvec PK(nth,njack),QK(nth,njack);
jvec P0(nth,njack),Q0(nth,njack),Q2(nth,njack),P2(nth,njack);
jvec P0_r(nth,njack),Q0_r(nth,njack),Q2_r(nth,njack),P2_r(nth,njack);
for(int ith=0;ith<nth;ith++)
{
P0[ith]=ED[ith]+EP[ith]; //P=initial+final
Q0[ith]=ED[ith]-EP[ith]; //Q=initial-final
P0_r[ith]=latt_en(ED[0],th_P[ith])+latt_en(EP[0],th_P[ith]);
Q0_r[ith]=latt_en(ED[0],th_P[ith])-latt_en(EP[0],th_P[ith]);
//we are describing the process D->Pi
mom[ith]=momentum(th_P[ith]);
double P_D=-mom[ith];
double P_Pi=mom[ith];
PK[ith]=P_D+P_Pi;
QK[ith]=P_D-P_Pi;
P2[ith]=sqr(P0[ith])-3*sqr(PK[ith]);
Q2[ith]=sqr(Q0[ith])-3*sqr(QK[ith]);
//reconstruct Q2
P2_r[ith]=sqr(P0_r[ith])-3*sqr(PK[ith]);
Q2_r[ith]=sqr(Q0_r[ith])-3*sqr(QK[ith]);
}
//checking Pion dispertion relation
ofstream out_disp_P(combine("plots/Pion_disp_rel_il_%d_ic_%d.xmg",il,ic).c_str());
out_disp_P<<"@type xydy"<<endl;
for(int ith=0;ith<nth;ith++) out_disp_P<<3*sqr(mom[ith])<<" "<<sqr(EP[ith])<<endl;
out_disp_P<<"&"<<endl;
for(int ith=0;ith<nth;ith++) out_disp_P<<3*sqr(mom[ith])<<" "<<sqr(cont_en(EP[0],th_P[ith]))<<endl;
out_disp_P<<"&"<<endl;
for(int ith=0;ith<nth;ith++) out_disp_P<<3*sqr(mom[ith])<<" "<<sqr(latt_en(EP[0],th_P[ith]))<<endl;
out_disp_P<<"&"<<endl;
//checking D dispertion relation
ofstream out_disp_D(combine("plots/D_disp_rel_il_%d_ic_%d.xmg",il,ic).c_str());
out_disp_D<<"@type xydy"<<endl;
for(int ith=0;ith<nth;ith++) out_disp_D<<3*sqr(mom[ith])<<" "<<sqr(ED[ith])<<endl;
out_disp_D<<"&"<<endl;
for(int ith=0;ith<nth;ith++) out_disp_D<<3*sqr(mom[ith])<<" "<<sqr(cont_en(ED[0],th_P[ith]))<<endl;
out_disp_D<<"&"<<endl;
for(int ith=0;ith<nth;ith++) out_disp_D<<3*sqr(mom[ith])<<" "<<sqr(latt_en(ED[0],th_P[ith]))<<endl;
out_disp_D<<"&"<<endl;
//compute xi
jvec xi(nth,njack);
for(int ith=1;ith<nth;ith++)
{
int E_fit_reco_flag=0; //it makes no diff
jack P0_th=E_fit_reco_flag?P0_r[ith]:P0[ith];
jack Q0_th=E_fit_reco_flag?Q0_r[ith]:Q0[ith];
xi[ith]=R2[ith]*P0_th;
xi[ith]/=QK[ith]-R2[ith]*Q0_th;
}
//compute fP
ofstream out_fP_r(combine("plots/fP_r_il_%d_ic_%d.xmg",il,ic).c_str());
out_fP_r<<"@type xydy"<<endl;
for(int ith=1;ith<nth;ith++)
{
int E_fit_reco_flag=1; //it makes no diff
jack P0_th=E_fit_reco_flag?P0_r[ith]:P0[ith];
jack Q0_th=E_fit_reco_flag?Q0_r[ith]:Q0[ith];
jack c=P0_th/(ED[0]+EP[0])*(1+xi[ith]*Q0_th/P0_th);
fP_r[ith]=R1[ith]/c*f0_r[0];
out_fP_r<<Q2[ith].med()<<" "<<fP_r[ith]<<endl;
}
//compute f0 and fT
ofstream out_f0_r(combine("plots/f0_r_il_%d_ic_%d.xmg",il,ic).c_str());
ofstream out_fT_r(combine("plots/fT_r_il_%d_ic_%d.xmg",il,ic).c_str());;
out_f0_r<<"@type xydy"<<endl;
out_f0_r<<Q2[0].med()<<" "<<f0_r[0]<<endl;
out_fT_r<<"@type xydy"<<endl;
for(int ith=1;ith<nth;ith++)
{
//it seems better here to solve using reconstructed energies
int E_fit_reco_flag=0;
jack EP_th=E_fit_reco_flag?latt_en(EP[0],th_P[ith]):EP[ith];
jack ED_th=E_fit_reco_flag?latt_en(ED[0],th_P[ith]):ED[ith];
jack Q2_th=E_fit_reco_flag?Q2_r[ith]:Q2[ith];
jack fM_r=xi[ith]*fP_r[ith]; //checked
f0_r[ith]=fP_r[ith]+fM_r[ith]*Q2_th/(sqr(ED_th)-sqr(EP_th));
out_f0_r<<Q2[ith].med()<<" "<<f0_r[ith]<<endl;
fT_r[ith]=fM_r[ith]*RT[ith]*Zt_med[ibeta]/Zv_med[ibeta]*(EP[0]+ED[0])/(ED[ith]+EP[ith]); //ADD
out_fT_r<<Q2[ith].med()<<" "<<fT_r[ith]<<endl;
}
//////////////////////////////////////// analytic method /////////////////////////////
jvec fP_a(nth,njack),fM_a(nth,njack),f0_a(nth,njack),fT_a(nth,njack);
jvec fP_n(nth,njack),fM_n(nth,njack),f0_n(nth,njack),fT_n(nth,njack);
//determine M and Z for pion and D
jvec ZP(nth,njack),ZD(nth,njack);
for(int ith=0;ith<nth;ith++)
{
jack E,Z2;
two_pts_fit(E,Z2,P_corr[ith],tmin_P,TH);
ZP[ith]=sqrt(Z2);
two_pts_fit(E,Z2,D_corr[ith],tmin_D,TH);
ZD[ith]=sqrt(Z2);
}
//compute V
jvec VK_a(nth,njack),V0_a(nth,njack),TK_a(nth,njack);
jvec VK_n(nth,njack),V0_n(nth,njack),TK_n(nth,njack);
for(int ith=0;ith<nth;ith++)
{
ofstream out_V0(combine("plots/V0_il_%d_ic_%d_ith_%d_analytic_numeric.xmg",il,ic,ith).c_str());
out_V0<<"@type xydy"<<endl;
ofstream out_VK(combine("plots/VK_il_%d_ic_%d_ith_%d_analytic_numeric.xmg",il,ic,ith).c_str());
out_VK<<"@type xydy"<<endl;
ofstream out_TK(combine("plots/TK_il_%d_ic_%d_ith_%d_analytic_numeric.xmg",il,ic,ith).c_str());
out_TK<<"@type xydy"<<endl;
ofstream out_dt(combine("plots/dt_il_%d_ic_%d_ith_%d.xmg",il,ic,ith).c_str());
out_dt<<"@type xydy"<<endl;
//computing time dependance
jvec dt_a(TH+1,njack),dt_n(TH+1,njack);
{
//it seems better here to use fitted energies
int E_fit_reco_flag=1;
jack EP_th=E_fit_reco_flag?latt_en(EP[0],th_P[ith]):EP[ith];
jack ED_th=E_fit_reco_flag?latt_en(ED[0],th_P[ith]):ED[ith];
for(int t=0;t<=TH;t++)
{
dt_a[t]=exp(-(ED_th*t+EP_th*(TH-t)))*ZP[0]*ZD[0]/(4*EP_th*ED_th);
dt_n[t]=D_corr[ith][t]*P_corr[ith][TH-t]/(ZD[0]*ZP[0]);
}
}
//remove time dependance using analytic or numeric expression
jvec VK_corr_a=Zv_med[ibeta]*lcK_th[ith]/dt_a,V0_corr_a=Zv_med[ibeta]*lc0_th[ith]/dt_a;
jvec VK_corr_n=Zv_med[ibeta]*lcK_th[ith]/dt_n,V0_corr_n=Zv_med[ibeta]*lc0_th[ith]/dt_n;
jvec TK_corr_n=Zt_med[ibeta]*lcT_th[ith]/dt_n,TK_corr_a=Zt_med[ibeta]*lcT_th[ith]/dt_a;
//fit V0
V0_a[ith]=constant_fit(V0_corr_a,tmin,tmax);
V0_n[ith]=constant_fit(V0_corr_n,tmin,tmax);
out_V0<<write_constant_fit_plot(V0_corr_a,V0_a[ith],tmin,tmax)<<"&"<<endl;
out_V0<<write_constant_fit_plot(V0_corr_n,V0_n[ith],tmin,tmax)<<"&"<<endl;
//fit VK
VK_a[ith]=constant_fit(VK_corr_a,tmin,tmax);
VK_n[ith]=constant_fit(VK_corr_n,tmin,tmax);
out_VK<<write_constant_fit_plot(VK_corr_a,VK_a[ith],tmin,tmax)<<"&"<<endl;
out_VK<<write_constant_fit_plot(VK_corr_n,VK_n[ith],tmin,tmax)<<"&"<<endl;
//fit TK
TK_a[ith]=constant_fit(TK_corr_a,tmin,tmax);
TK_n[ith]=constant_fit(TK_corr_n,tmin,tmax);
out_TK<<write_constant_fit_plot(TK_corr_a,TK_a[ith],tmin,tmax)<<"&"<<endl;
out_TK<<write_constant_fit_plot(TK_corr_n,TK_n[ith],tmin,tmax)<<"&"<<endl;
}
//compute f0(q2max)
f0_a[0]=V0_a[0]/(ED[0]+EP[0]);
f0_n[0]=V0_n[0]/(ED[0]+EP[0]);
cout<<"f0_a["<<Q2[0].med()<<"]: "<<f0_a[0]<<endl;
cout<<"f0_n["<<Q2[0].med()<<"]: "<<f0_n[0]<<endl;
//solve for fP and f0
for(int ith=1;ith<nth;ith++)
{
jack delta=P0[ith]*QK[ith]-Q0[ith]*PK[ith];
//solve using analytic fit
jack deltaP_a=V0_a[ith]*QK[ith]-Q0[ith]*VK_a[ith];
jack deltaM_a=P0[ith]*VK_a[ith]-V0_a[ith]*PK[ith];
fP_a[ith]=deltaP_a/delta;
fM_a[ith]=deltaM_a/delta;
//solve using numeric fit
jack deltaP_n=V0_n[ith]*QK[ith]-Q0[ith]*VK_n[ith];
jack deltaM_n=P0[ith]*VK_n[ith]-V0_n[ith]*PK[ith];
fP_n[ith]=deltaP_n/delta;
fM_n[ith]=deltaM_n/delta;
//compute f0
f0_a[ith]=fP_a[ith]+fM_a[ith]*Q2[ith]/(ED[0]*ED[0]-EP[0]*EP[0]);
f0_n[ith]=fP_n[ith]+fM_n[ith]*Q2[ith]/(ED[0]*ED[0]-EP[0]*EP[0]);
//solve fT
fT_a[ith]=-TK_a[ith]*(EP[0]+ED[0])/(2*(ED[ith]+EP[ith]))/mom[ith];
fT_n[ith]=-TK_n[ith]*(EP[0]+ED[0])/(2*(ED[ith]+EP[ith]))/mom[ith];
}
//write analytic and umeric plot of fP and f0
ofstream out_fP_a("plots/fP_a.xmg"),out_fP_n("plots/fP_n.xmg");
ofstream out_fM_a("plots/fM_a.xmg"),out_fM_n("plots/fM_n.xmg");
ofstream out_f0_a("plots/f0_a.xmg"),out_f0_n("plots/f0_n.xmg");
ofstream out_fT_a("plots/fT_a.xmg"),out_fT_n("plots/fT_n.xmg");
out_fP_a<<"@type xydy"<<endl;
out_fP_n<<"@type xydy"<<endl;
out_f0_a<<"@type xydy"<<endl;
out_f0_n<<"@type xydy"<<endl;
out_fM_a<<"@type xydy"<<endl;
out_fM_n<<"@type xydy"<<endl;
out_fT_a<<"@type xydy"<<endl;
out_fT_n<<"@type xydy"<<endl;
out_f0_a<<Q2[0].med()<<" "<<f0_a[0]<<endl;
out_f0_n<<Q2[0].med()<<" "<<f0_n[0]<<endl;
for(int ith=1;ith<nth;ith++)
{
out_fP_a<<Q2[ith].med()<<" "<<fP_a[ith]<<endl;
out_fP_n<<Q2[ith].med()<<" "<<fP_n[ith]<<endl;
out_fM_a<<Q2[ith].med()<<" "<<fM_a[ith]<<endl;
out_fM_n<<Q2[ith].med()<<" "<<fM_n[ith]<<endl;
out_f0_a<<Q2[ith].med()<<" "<<f0_a[ith]<<endl;
out_f0_n<<Q2[ith].med()<<" "<<f0_n[ith]<<endl;
out_fT_a<<Q2[ith].med()<<" "<<fT_a[ith]<<endl;
out_fT_n<<Q2[ith].med()<<" "<<fT_n[ith]<<endl;
}
ext_EP=EP;
ext_ED=ED;
ext_Q2=Q2;
ext_fP=fP_a;
ext_fM=fM_a;
ext_f0=f0_a;
ext_fT=fT_a;
}
/***********************************************************
when update npc position
***********************************************************/
void ExternalActor::NpcChangedUpdate(double updatetime,
float CurrPosX, float CurrPosY, float CurrPosZ,
float CurrRotation, const std::string &CurrAnimation,
bool ResetPosition, bool ResetRotation,
const LbaNet::PlayingSoundSequence &Sounds,
LbaNet::NpcUpdateBasePtr Update,
ScriptEnvironmentBase* scripthandler)
{
// reset free move
_freemove = false;
// reset projectiles
if(_character)
_character->ClearActionsOnAnimation();
// update only newest info
if(updatetime < _last_update)
return;
_last_update = updatetime;
if(_playingscript)
return;
boost::shared_ptr<PhysicalObjectHandlerBase> physo = _character->GetPhysicalObject();
float posX, posY, posZ;
physo->GetPosition(posX, posY, posZ);
float rotation = physo->GetRotationYAxis();
// update position and rotation
float diffpos = (CurrPosX-posX)*(CurrPosX-posX) +
(CurrPosY-posY)*(CurrPosY-posY) +
(CurrPosZ-posZ)*(CurrPosZ-posZ);
float diffrot = abs(CurrRotation-rotation);
// reset actor position
if(ResetPosition || diffpos > 64 || _shouldreset)
{
physo->SetPosition(CurrPosX, CurrPosY, CurrPosZ);
posX = CurrPosX;
posY = CurrPosY;
posZ = CurrPosZ;
}
// reset actor rotation
if(ResetRotation || diffrot > 20 || _shouldreset)
{
LbaQuaternion Q(CurrRotation, LbaVec3(0,1,0));
physo->SetRotation(Q);
rotation = CurrRotation;
}
_differencePosX = CurrPosX-posX;
_differencePosY = CurrPosY-posY;
_differencePosZ = CurrPosZ-posZ;
_differenceRotation = CurrRotation-rotation;
//update animation
if(CurrAnimation != "")
_character->GetDisplayObject()->Update(new LbaNet::AnimationStringUpdate(CurrAnimation), _playingscript);
_shouldreset = false;
// update sounds
boost::shared_ptr<SoundObjectHandlerBase> soundo = _character->GetSoundObject();
if(soundo)
{
soundo->SetSoundVector(Sounds, false);
_character->UpdateSoundPosition();
}
// update the script part
if(!Update)
{
_currentScripts = boost::shared_ptr<ScriptPartBase>();
return;
}
LbaNet::NpcUpdateBase & obj = *Update;
const std::type_info& info = typeid(obj);
// StraightWalkToNpcUpd
if(info == typeid(LbaNet::StraightWalkToNpcUpd))
{
LbaNet::StraightWalkToNpcUpd * castedptr =
dynamic_cast<LbaNet::StraightWalkToNpcUpd *>(&obj);
_currentScripts = boost::shared_ptr<ScriptPartBase>(new
StraightWalkToScriptPart(0, false, castedptr->PosX, castedptr->PosY, castedptr->PosZ,
_character));
return;
}
if(info == typeid(LbaNet::WalkToPointNpcUpd))
{
LbaNet::WalkToPointNpcUpd * castedptr =
dynamic_cast<LbaNet::WalkToPointNpcUpd *>(&obj);
_currentScripts = boost::shared_ptr<ScriptPartBase>(new
WalkToPointScriptPart(0, false, castedptr->PosX, castedptr->PosY, castedptr->PosZ, castedptr->RotationSpeedPerSec, castedptr->moveForward));
return;
}
// GoToNpcUpd
if(info == typeid(LbaNet::GoToNpcUpd))
{
LbaNet::GoToNpcUpd * castedptr =
dynamic_cast<LbaNet::GoToNpcUpd *>(&obj);
_currentScripts = boost::shared_ptr<ScriptPartBase>(new
GoToScriptPart(0, false, castedptr->PosX, castedptr->PosY, castedptr->PosZ, castedptr->Speed,
_character));
return;
}
// RotateNpcUpd
if(info == typeid(LbaNet::RotateNpcUpd))
{
LbaNet::RotateNpcUpd * castedptr =
dynamic_cast<LbaNet::RotateNpcUpd *>(&obj);
_currentScripts = boost::shared_ptr<ScriptPartBase>(new
RotateScriptPart(0, false, castedptr->Angle, castedptr->RotationSpeedPerSec,
castedptr->ManageAnimation));
return;
}
// AnimateNpcUpd
if(info == typeid(LbaNet::AnimateNpcUpd))
{
LbaNet::AnimateNpcUpd * castedptr =
dynamic_cast<LbaNet::AnimateNpcUpd *>(&obj);
_currentScripts = boost::shared_ptr<ScriptPartBase>(new
PlayAnimationScriptPart(0, false, castedptr->AnimationMove, castedptr->NbAnimation));
return;
}
// RotateFromPointNpcUpd
if(info == typeid(LbaNet::RotateFromPointNpcUpd))
{
LbaNet::RotateFromPointNpcUpd * castedptr =
dynamic_cast<LbaNet::RotateFromPointNpcUpd *>(&obj);
_currentScripts = boost::shared_ptr<ScriptPartBase>(new
RotateFromPointScriptPart(0, false, castedptr->Angle,
castedptr->PosX, castedptr->PosY, castedptr->PosZ, castedptr->Speed, _character));
return;
}
// FollowWaypointNpcUpd
if(info == typeid(LbaNet::FollowWaypointNpcUpd))
{
LbaNet::FollowWaypointNpcUpd * castedptr =
dynamic_cast<LbaNet::FollowWaypointNpcUpd *>(&obj);
LbaVec3 Pm1X(castedptr->Pm1X, castedptr->Pm1Y, castedptr->Pm1Z);
LbaVec3 P0(castedptr->P0X, castedptr->P0Y, castedptr->P0Z);
LbaVec3 P1(castedptr->P1X, castedptr->P1Y, castedptr->P1Z);
LbaVec3 P2(castedptr->P2X, castedptr->P2Y, castedptr->P2Z);
LbaVec3 P3(castedptr->P3X, castedptr->P3Y, castedptr->P3Z);
LbaVec3 P4(castedptr->P4X, castedptr->P4Y, castedptr->P4Z);
_currentScripts = boost::shared_ptr<ScriptPartBase>(new
FollowWaypointScriptPart(0, false, Pm1X, P0, P1, P2, P3, P4));
return;
}
}
