// preserve orientation of the most anisotropic metric in 2D!!!
SMetric3 intersection_conserve_mostaniso_2d (const SMetric3 &m1, const SMetric3 &m2)
{
fullMatrix<double> V1(3,3);
fullVector<double> S1(3);
m1.eig(V1,S1,false);
double ratio1 = anisoRatio2D(V1(0,0),V1(1,0),V1(2,0),
V1(0,1),V1(1,1),V1(2,1),
V1(0,2),V1(1,2),V1(2,2),
S1(0),S1(1),S1(2));
fullMatrix<double> V2(3,3);
fullVector<double> S2(3);
m2.eig(V2,S2,false);
double ratio2 = anisoRatio2D(V2(0,0),V2(1,0),V2(2,0),
V2(0,1),V2(1,1),V2(2,1),
V2(0,2),V2(1,2),V2(2,2),
S2(0),S2(1),S2(2));
if (ratio1 < ratio2)
return intersection_conserveM1(m1, m2);
else
return intersection_conserveM1(m2, m1);
}
arma::fmat CollisionRespone::fdirection(int d,arma::fmat A,std::list<int> C){
int i=0,j=0;
arma::fmat delta_f(ActiveCollisionList.size(),1),V1(C.size(),1),A_11(C.size(),C.size()),X;
delta_f.zeros();
delta_f(d,0)=1;
// std::cout<<"matrix delta_f\n"<<delta_f<<"\n";
if(C.size()!=0){
for(std::list<int>::iterator it=C.begin();it!=C.end();it++){
j=0;
for(std::list<int>::iterator it2=C.begin();it2!=C.end();it2++){
//std::cout<<"A_11 accesing "<<*it<<","<<*it2<<" at "<<i<<","<<j<<"\n";
A_11(i,j)=A(*it,*it2);
j++;
}
V1(i,0)=A(*it,d);
i++;
}
X=-inv(A_11)*V1;
i=0;
// std::cout<<"matrix x"<<X<<"\n";
for(std::list<int>::iterator it=C.begin();it!=C.end();it++){
// std::cout<<"inserting into "<<*it<<"\n";
delta_f(*it,0)=X(i,0);
i++;
}
}
//std::cout<<"matrix delta_f\n"<<delta_f<<"\n";
return delta_f;
}
void Box::create(Vector3 origin, float length, float depth, float width)
{
this->center = origin;
this->sizeX = length;
this->sizeY = depth;
this->sizeZ = width;
Vector3 V0(origin.x()-(length/2), origin.y()-(depth/2), origin.z()-(width/2));
Vector3 V1(origin.x()+(length/2), origin.y()-(depth/2), origin.z()-(width/2));
Vector3 V2(origin.x()+(length/2), origin.y()+(depth/2), origin.z()-(width/2));
Vector3 V3(origin.x()-(length/2), origin.y()+(depth/2), origin.z()-(width/2));
Vector3 V4(origin.x()-(length/2), origin.y()-(depth/2), origin.z()+(width/2));
Vector3 V5(origin.x()+(length/2), origin.y()-(depth/2), origin.z()+(width/2));
Vector3 V6(origin.x()+(length/2), origin.y()+(depth/2), origin.z()+(width/2));
Vector3 V7(origin.x()-(length/2), origin.y()+(depth/2), origin.z()+(width/2));
vector<Quadrilateral> Q;
Q.push_back(Quadrilateral(V0,V1,V5,V4));
Q.push_back(Quadrilateral(V2,V3,V7,V6));
Q.push_back(Quadrilateral(V0,V4,V7,V3));
Q.push_back(Quadrilateral(V5,V1,V2,V6));
Q.push_back(Quadrilateral(V1,V0,V3,V2));
Q.push_back(Quadrilateral(V4,V5,V6,V7));
setFaces(Q);
calculateMinMax();
}
static void WriteBootloaderMsg( WORD msg, WORD key )
{
V1( EventLogAdd( EVENTLOG_UPDATER_WRITE_MSG ) );
FlashEraseSegment( (WORD)&BootloaderMsg );
FlashWrite( (WORD)&BootloaderMsg.msg, (DWORD)msg, FLASH_WRITE_WORD );
FlashWrite( (WORD)&BootloaderMsg.key, (DWORD)key, FLASH_WRITE_WORD );
}
void
vpMbtDistanceKltCylinder::buildFrom(const vpPoint &p1, const vpPoint &p2, const double &r)
{
p1Ext = p1;
p2Ext = p2;
vpColVector ABC(3);
vpColVector V1(3);
vpColVector V2(3);
V1[0] = p1.get_oX();
V1[1] = p1.get_oY();
V1[2] = p1.get_oZ();
V2[0] = p2.get_oX();
V2[1] = p2.get_oY();
V2[2] = p2.get_oZ();
// Get the axis of the cylinder
ABC = V1-V2;
// Build our extremity circles
circle1.setWorldCoordinates(ABC[0],ABC[1],ABC[2],p1.get_oX(),p1.get_oY(),p1.get_oZ(),r);
circle2.setWorldCoordinates(ABC[0],ABC[1],ABC[2],p2.get_oX(),p2.get_oY(),p2.get_oZ(),r);
// Build our cylinder
cylinder.setWorldCoordinates(ABC[0],ABC[1],ABC[2],(p1.get_oX()+p2.get_oX())/2.0,(p1.get_oY()+p2.get_oY())/2.0,(p1.get_oZ()+p2.get_oZ())/2.0,r);
}
void UpdateMap(vec_zz_p& x, const vec_zz_p& aa,
const zz_pXMultiplier& B, const zz_pXModulus& F)
{
long n = F.n;
vec_zz_p a;
a = aa;
StripZeroes(a);
if (a.length() > n) Error("UpdateMap: bad args");
long i;
if (!B.UseFFT) {
PlainUpdateMap(x, a, B.b, F.f);
StripZeroes(x);
return;
}
fftRep R1(INIT_SIZE, F.k), R2(INIT_SIZE, F.l);
vec_zz_p V1(INIT_SIZE, n);
RevTofftRep(R1, a, F.k, 0, a.length()-1, 0);
mul(R2, R1, F.FRep);
RevFromfftRep(V1, R2, 0, n-2);
for (i = 0; i <= n-2; i++) negate(V1[i], V1[i]);
RevTofftRep(R2, V1, F.l, 0, n-2, n-1);
mul(R2, R2, B.B1);
mul(R1, R1, B.B2);
AddExpand(R2, R1);
RevFromfftRep(x, R2, 0, n-1);
StripZeroes(x);
}
//2線分間の距離
float CCollision::DistanceLine(const CVector3D &s1,const CVector3D &e1,const CVector3D &s2,const CVector3D &e2,CVector3D *c,CVector3D *c2){
//線のベクトルを求める
CVector3D V1(e1-s1);
CVector3D V2(e2-s2);
//2つの線分の外積を求める
CVector3D N(CVector3D::Cross(V1,V2).GetNormalize());
//2つの線分が平行でない場合
if (N.LengthSq()) {
float l = CVector3D::Dot(N, s1-s2);
CVector3D m = N*l;
//線分２を線分１と同じ同一平面上に合わせる
CVector3D S2 = s2+m;
CVector3D E2 = e2+m;
//交差しているか調べる
float d1=CVector3D::Dot(CVector3D::Cross(V1,s1-S2),CVector3D::Cross(V1,s1-E2));
float d2=CVector3D::Dot(CVector3D::Cross(V2,S2-s1),CVector3D::Cross(V2,S2-e1));
if( d1 < 0 &&
d2 < 0 ) {
return abs(l);
}
}
//始点と終点から線分との距離を求め、もっとも短い距離を返す
return sqrt(min(
min(DistancePointToLineSq(s2, e2, s1),
DistancePointToLineSq(s2, e2, e1)),
min(DistancePointToLineSq(s1, e1, s2),
DistancePointToLineSq(s1, e1, e2))
));
}
void TouchupStyleLayer::draw(Way* R)
{
const FeaturePainter* paintsel = R->getPainter(r->thePixelPerM);
if (paintsel)
paintsel->drawTouchup(R,r->thePainter,r);
else if (!R->hasPainter() && !TEST_RENDERER_RFLAGS(RendererOptions::UnstyledHidden)) {
if ( r->theOptions.arrowOptions != RendererOptions::ArrowsNever )
{
Feature::TrafficDirectionType TT = trafficDirection(R);
if ( (TT != Feature::UnknownDirection) || (r->theOptions.arrowOptions == RendererOptions::ArrowsAlways) )
{
qreal theWidth = r->thePixelPerM*R->widthOf()-4;
if (theWidth > 8)
theWidth = 8;
qreal DistFromCenter = 2*(theWidth+4);
if (theWidth > 0)
{
for (int i=1; i<R->size(); ++i)
{
QPointF FromF(r->toView(R->getNode(i-1)));
QPointF ToF(r->toView(R->getNode(i)));
if (distance(FromF,ToF) > (DistFromCenter*2+4))
{
QPointF H(FromF+ToF);
H *= 0.5;
qreal A = angle(FromF-ToF);
QPointF T(DistFromCenter*cos(A),DistFromCenter*sin(A));
QPointF V1(theWidth*cos(A+M_PI/6),theWidth*sin(A+M_PI/6));
QPointF V2(theWidth*cos(A-M_PI/6),theWidth*sin(A-M_PI/6));
if ( (TT == Feature::OtherWay) || (TT == Feature::BothWays) )
{
r->thePainter->setPen(QPen(QColor(0,0,255), 2));
r->thePainter->drawLine(H+T,H+T-V1);
r->thePainter->drawLine(H+T,H+T-V2);
}
if ( (TT == Feature::OneWay) || (TT == Feature::BothWays) )
{
r->thePainter->setPen(QPen(QColor(0,0,255), 2));
r->thePainter->drawLine(H-T,H-T+V1);
r->thePainter->drawLine(H-T,H-T+V2);
}
else
{
if ( r->theOptions.arrowOptions == RendererOptions::ArrowsAlways )
{
r->thePainter->setPen(QPen(QColor(255,0,0), 2));
r->thePainter->drawLine(H-T,H-T+V1);
r->thePainter->drawLine(H-T,H-T+V2);
}
}
}
}
}
}
}
}
}
void CatmullRomCurveEvaluator::convertPoints(std::vector<Point>& pts, Point P0, Point P1, Point P2, Point P3) const {
Point V0(P1);
Point V1(Point(P1.x + cat / 3 * (P2.x - P0.x), P1.y + cat / 3 * (P2.y - P0.y)));
Point V2(Point(P2.x - cat / 3 * (P3.x - P1.x), P2.y - cat / 3 * (P3.y - P1.y)));
Point V3(P2);
pts.push_back(V0);
pts.push_back(V1);
pts.push_back(V2);
pts.push_back(V3);
}
void
osx_driver_setclipboard_loop(
struct ts_display_t *d,
ts_clipboard_p clipboard)
{
if (!clip)
PasteboardCreate(kPasteboardClipboard, &clip);
if (!clipboard->flavorCount)
return;
for (int i = 0; i < clipboard->flavorCount; i++) {
if (!strcmp(clipboard->flavor[i].name, "text")) {
V1("%s adding %d bytes of %s\n", __func__,
(int)clipboard->flavor[i].size, clipboard->flavor[i].name);
if (PasteboardClear(clip) != noErr) {
V1("apple pasteboard clear failed");
return;
}
PasteboardSyncFlags flags = PasteboardSynchronize(clip);
if ((flags & kPasteboardModified) || !(flags & kPasteboardClientIsOwner)) {
V1("apple pasteboard cannot assert ownership");
return;
}
CFDataRef cfdata = CFDataCreate(kCFAllocatorDefault,
(uint8_t*)clipboard->flavor[i].data,
clipboard->flavor[i].size);
if (cfdata == nil) {
V1("apple pasteboard cfdatacreate failed");
return;
}
if (PasteboardPutItemFlavor(clip, (PasteboardItemID) 1,
CFSTR("public.utf8-plain-text"), cfdata, 0) != noErr) {
V1("apple pasteboard putitem failed");
CFRelease(cfdata);
}
CFRelease(cfdata);
return;
}
}
}
/*-------------------------------------------------------------------------*
* New_Alpha *
* *
* Returns a new spherical triangle derived from the original one by *
* moving the "C" vertex along the edge "BC" until the new "alpha" angle *
* equals the given argument. *
* *
*-------------------------------------------------------------------------*/
SphericalTriangle SphericalTriangle::New_Alpha( double alpha ) const
{
Vector3d V1( A() ), V2( B() ), V3( C() );
Vector3d E1 = Unit( V2 ^ V1 );
Vector3d E2 = E1 ^ V1;
Vector3d G = ( cos(alpha) * E1 ) + ( sin(alpha) * E2 );
Vector3d D = Unit( V3 / V2 );
Vector3d C2 = ((G * D) * V2) - ((G * V2) * D);
if( Triple( V1, V2, C2 ) > 0.0 ) C2 = -1.0 * C2 ;
return SphericalTriangle( V1, V2, C2 );
}
/*-------------------------------------------------------------------------*
* New_Alpha *
* *
* Returns a new spherical triangle derived from the original one by *
* moving the "C" vertex along the edge "BC" until the new "alpha" angle *
* equals the given argument. *
* *
*-------------------------------------------------------------------------*/
SphericalTriangle SphericalTriangle::New_Alpha( float alpha ) const
{
Vec3 V1( A() ), V2( B() ), V3( C() );
Vec3 E1 = Unit( V2 ^ V1 );
Vec3 E2 = E1 ^ V1;
Vec3 G = ( cos(alpha) * E1 ) + ( sin(alpha) * E2 );
Vec3 D = Unit( V3 / V2 );
Vec3 C2 = ((G * D) * V2) - ((G * V2) * D);
if( Triple( V1, V2, C2 ) > 0.0 ) C2 *= -1.0;
return SphericalTriangle( V1, V2, C2 );
}
/*!
Build a vpMbtDistanceLine thanks to two points corresponding to the extremities.
\param _p1 : The first extremity.
\param _p2 : The second extremity.
*/
void
vpMbtDistanceLine::buildFrom(vpPoint &_p1, vpPoint &_p2)
{
line = new vpLine ;
poly.setNbPoint(2);
poly.addPoint(0, _p1);
poly.addPoint(1, _p2);
p1 = &poly.p[0];
p2 = &poly.p[1];
vpColVector V1(3);
vpColVector V2(3);
vpColVector V3(3);
vpColVector V4(3);
V1[0] = p1->get_oX();
V1[1] = p1->get_oY();
V1[2] = p1->get_oZ();
V2[0] = p2->get_oX();
V2[1] = p2->get_oY();
V2[2] = p2->get_oZ();
//if((V1-V2).sumSquare()!=0)
if(std::fabs((V1-V2).sumSquare()) > std::numeric_limits<double>::epsilon())
{
{
V3[0]=double(rand()%1000)/100;
V3[1]=double(rand()%1000)/100;
V3[2]=double(rand()%1000)/100;
vpColVector v_tmp1,v_tmp2;
v_tmp1 = V2-V1;
v_tmp2 = V3-V1;
V4=vpColVector::cross(v_tmp1,v_tmp2);
}
vpPoint P3;
P3.setWorldCoordinates(V3[0],V3[1],V3[2]);
vpPoint P4;
P4.setWorldCoordinates(V4[0],V4[1],V4[2]);
buildLine(*p1,*p2, P3,P4, *line) ;
}
else
{
vpPoint P3;
P3.setWorldCoordinates(V1[0],V1[1],V1[2]);
vpPoint P4;
P4.setWorldCoordinates(V2[0],V2[1],V2[2]);
buildLine(*p1,*p2,P3,P4,*line) ;
}
}
Embedding MNF(const Matrix &data, const Matrix &noise_covariance, Index reduced_dimensions) {
if (noise_covariance.cols() != noise_covariance.rows() || noise_covariance.rows() != data.cols()) {
throw std::invalid_argument("The rows and columns of noise covariance should both equal to the columns of the data.");
}
if (reduced_dimensions == 0) reduced_dimensions = data.cols();
if (reduced_dimensions > data.cols()) throw std::invalid_argument("Reduced dimensions should be less than or equal to the total dimensions.");
Matrix U1(noise_covariance);
Matrix V1(data.cols(), data.cols());
gsl_vector *S1v = gsl_vector_alloc(data.cols());
gsl_linalg_SV_decomp_jacobi(U1.m_, V1.m_, S1v);
Matrix wX(data.rows(), data.cols());
Matrix wXintermediate(data.cols(), data.cols());
Matrix invsqrtS1(data.cols(), data.cols());
for (Index i = 0; i < data.cols(); ++i) {
invsqrtS1(i, i) = 1.0 / sqrt(gsl_vector_get(S1v, i));
}
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, U1.m_, invsqrtS1.m_, 0.0, wXintermediate.m_);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, data.m_, wXintermediate.m_, 0.0, wX.m_);
gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, wX.m_, wX.m_, 0.0, wXintermediate.m_);
Matrix V2(data.cols(), data.cols());
gsl_vector *S2v = gsl_vector_alloc(data.cols());
gsl_linalg_SV_decomp_jacobi(wXintermediate.m_, V2.m_, S2v);
Embedding result;
result.space = std::make_shared<gsl::Matrix>(data.rows(), reduced_dimensions);
result.vectors = std::make_shared<gsl::Matrix>(data.cols(), data.cols());
result.values = std::make_shared<gsl::Matrix>(1, data.cols());
Matrix intermediate2(data.cols(), data.cols());
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, invsqrtS1.m_, V2.m_, 0.0, intermediate2.m_);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, U1.m_, intermediate2.m_, 0.0, result.vectors->m_);
gsl_matrix_const_view reduced_vectors = gsl_matrix_const_submatrix(result.vectors->m_, 0, 0, result.vectors->rows(), reduced_dimensions);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, data.m_, &reduced_vectors.matrix, 0.0, result.space->m_);
// TODO: assign eigenvalues and eigenvectors
gsl_vector_free(S1v);
gsl_vector_free(S2v);
return result;
}
void CP_Box::Draw(Vec3D<>* Envelope)
{
glPushMatrix();
if (Envelope) glScaled(Envelope->x, Envelope->y, Envelope->z);
Vec3D<> V1(X, Y, Z);
Vec3D<> V2(X+dX, Y+dY, Z+dZ);
CColor c(R, G, B, alpha);
CGL_Utils::DrawCube(V1, V2, true, true, 2.0, c);
glPopMatrix();
}
void Sub(double *u)
{
double temp1 = V1(u[0],u[1],u[2],u[3]);
double temp2 = V2(u[0],u[1],u[2],u[3]);
double temp3 = V3(u[0],u[1],u[2],u[3]);
double temp4 = V4(u[0],u[1],u[2],u[3]);
u[0] = temp1;
u[1] = temp2;
u[2] = temp3;
u[3] = temp4;
}
NaGePlane::NaGePlane(const NaGePoint3D& P1, const NaGePoint3D& P2, const NaGePoint3D& P3)
{
NaGeVector3D V1(P1, P2);
NaGeVector3D V2(P1, P3);
NaGeVector3D Dir = V1^V2;
NaGeAxisSystem ax(P1, Dir, V1);
itsLocation = ax;
UpdateEquation();
geomType = GEPLANE;
}
void Sub(int *u, int m)
{
int temp1 = V1(u[0],u[1],u[2],u[3], m);
int temp2 = V2(u[0],u[1],u[2],u[3], m);
int temp3 = V3(u[0],u[1],u[2],u[3], m);
int temp4 = V4(u[0],u[1],u[2],u[3], m);
u[0] = temp1;
u[1] = temp2;
u[2] = temp3;
u[3] = temp4;
}
main( )
{
VectorOf<foo> V1(1);
cout << "V1 = " << V1 << endl;
VectorOf<foo> V2 = V1;
cout << "V2 = " << V2 << endl;
((foo&)V2[0]).bar();
cout << "V1 = " << V1 << endl;
cout << "V2 = " << V2 << endl;
}
void GetPseudoObs(int *families, std::vector<double>& thetas, int *rotations, double *V, int d, unsigned int n)
{
std::vector<int> NumbParams(d-1);
std::vector<double> U1(n),V1(n);
int i;
int J=0;
for (i=d-1;i>=1;i--)
{
switch(families[i-1]){
case 0:
{
break;
}
case 18:
{
J += 3;
break;
}
case 3: case 4: case 5: case 6: case 12: case 16: case 17: case 19:
{
J += 2;
break;
}
default:
{
J += 1;
}
}
NumbParams[i-1] = J;
}
int TotalNumbParams = thetas.size();
for (i=d-1;i>=1;i--)
{
if (families[i-1] != 0)
{
if (rotations[i-1]>0)
{
Rotate_Obs(&V[0], &V[i*n],&U1[0],&V1[0],rotations[i-1],n);
PairCopulaVfun_Rotated_Obs(families[i-1], rotations[i-1], &thetas[TotalNumbParams-NumbParams[i-1]], &U1[0], &V1[0] , &V[i*n], n);
}
else
{
PairCopulaVfun(families[i-1], &thetas[TotalNumbParams-NumbParams[i-1]], &V[0], &V[i*n], &V[i*n], n);
}
}
}
}
void test_op()
{
Point_E3d P1(3.0,4.0,5.0);
Point_E3d P2(4.0,7.0,9.0);
Vector_E3d V1(8.0,4.0,2.0);
Point_E3d A1 = P1;
A1 += V1;
assert( Point_E3d(11.0,8.0,7.0) == A1 );
A1 -= V1;
assert( P1 == A1 );
Vector_E3d V2(P1, P2);
assert( Vector_E3d(1.0, 3.0, 4.0) == V2 );
}
// preserve orientation of the most anisotropic metric !!!
SMetric3 intersection_conserve_mostaniso (const SMetric3 &m1, const SMetric3 &m2)
{
fullMatrix<double> V1(3,3);
fullVector<double> S1(3);
m1.eig(V1,S1,true);
double ratio1 = fabs(S1(0)/S1(2)); // Minimum ratio because we take sorted eigenvalues
fullMatrix<double> V2(3,3);
fullVector<double> S2(3);
m2.eig(V2,S2,true);
double ratio2 = fabs(S2(0)/S2(2)); // Minimum ratio because we take sorted eigenvalues
if (ratio1 < ratio2)
return intersection_conserveM1(m1, m2);
else
return intersection_conserveM1(m2, m1);
}
void test_Transformation_E3()
{
Point_E3d P1(5,7,9);
Vector_E3d V1(11,13,17);
Point_E3d P2 = P1 + V1;
Transformation_E3d T(ORTHOGONAL, Point_E3d(4,5,6), Point_E3d(7,8,9));
Point_E3d P1t = T(P1);
Vector_E3d V1t = T(V1);
Point_E3d P2t = T(P2);
assert( P2t == P1t + V1t );
}
static Standard_Boolean TriangleIsValid(const gp_Pnt& P1, const gp_Pnt& P2, const gp_Pnt& P3)
{
gp_Vec V1(P1,P2);
gp_Vec V2(P2,P3);
gp_Vec V3(P3,P1);
if ((V1.SquareMagnitude() > 1.e-10) && (V2.SquareMagnitude() > 1.e-10) && (V3.SquareMagnitude() > 1.e-10)) {
V1.Cross(V2);
if (V1.SquareMagnitude() > 1.e-10)
return Standard_True;
else
return Standard_False;
}
else
return Standard_False;
}
Standard_Boolean Surface::triangleIsValid(const gp_Pnt& P1, const gp_Pnt& P2, const gp_Pnt& P3)
{
gp_Vec V1(P1,P2); // V1=(P1,P2)
gp_Vec V2(P2,P3); // V2=(P2,P3)
gp_Vec V3(P3,P1); // V3=(P3,P1)
if ((V1.SquareMagnitude() > 1.e-10) && (V2.SquareMagnitude() > 1.e-10) && (V3.SquareMagnitude() > 1.e-10))
{
gp_Vec normal = V1.Crossed(V2) + V2.Crossed(V3) + V3.Crossed(V1);
if (normal.SquareMagnitude() > 1.e-10)
return Standard_True;
else
return Standard_False;
}
return Standard_False;
}
void SmtWater::UpdateNormal()
{
//new mem
Vector3 *pNormals = new Vector3[CST_INT_GRID_SIZE];
//calculator normal
for( int iY=0; iY<CST_INT_GRID_HEIGHT - 1; iY++ )
{
for(int iX=0; iX<CST_INT_GRID_WIDTH - 1; iX++ )
{
int P1,P2,P3,P4;
P1 = (iY * CST_INT_GRID_WIDTH) + iX;
P2 = ((iY + 1) * CST_INT_GRID_WIDTH) + iX;
P3 = P1 + 1;
P4 = P2 + 1;
Vector4 V1(wvertex[P1].x,wvertex[P1].y,wvertex[P1].z),
V2(wvertex[P2].x,wvertex[P2].y,wvertex[P2].z),
V3(wvertex[P3].x,wvertex[P3].y,wvertex[P3].z),
V4(wvertex[P4].x,wvertex[P4].y,wvertex[P4].z);
Vector4 nor1 = GalcTriangleNormal(V1, V3, V2);
Vector4 nor2 = GalcTriangleNormal(V3, V4, V2);
pNormals[P1] += nor1;
pNormals[P2] += nor1;
pNormals[P3] += nor1;
pNormals[P2] += nor2;
pNormals[P3] += nor2;
pNormals[P4] += nor2;
}
}
//normalize
for(long i=0; i < CST_INT_GRID_SIZE; i++)
{
pNormals[i].Normalize();
wvertex[i].nx = pNormals[i].x;
wvertex[i].ny = pNormals[i].y;
wvertex[i].nz = pNormals[i].z;
}
SMT_SAFE_DELETE_A(pNormals);
}
void CodeUpdateStateInit( void )
{
ApiRegisterHandlers( DEVID_UPDATER, UpdaterRegRead, UpdaterRegWrite, UpdaterStartReceived, UpdaterStopReceived );
V1( EventLogAdd( EVENTLOG_UPDATER_INIT ) );
// set flag if our APP section is the correct one that goes with the current BOOT section.
AppCallsAllowed = IsImageValid( (ImageInfo *)RocketMetrics.AppImage,
RocketMetrics.BootImage->pairCrc,
RocketMetrics.BootImage->crc );
// if the CRC's match up ok, lets make sure the versions are equal too
if( AppCallsAllowed ) {
if( RocketMetrics.BootImage->version != RocketMetrics.AppImage->version ) {
AppCallsAllowed = FALSE;
}
}
}
void dataset::calc_dist(REALNUM_TYPE rtDef, REALNUM_TYPE metricV, UNSIGNED_1B_TYPE metricKind)
//metricKind: 0 - Euclidean, 1 - Cosine-based, 2 - Correlation coff, 3 - Tanimoto
//all coefficients are turned into distances in a way of = 1 - (coff)^metricV
{
UNSIGNED_4B_TYPE i, i1, j, N = patt.RowNO(), D = patt.ColNO();
dist.SetSize(N, N);
apvector<REALNUM_TYPE> V1(D), V2(D);
for (i = 0; i < N; i++)
{
for (j = 0; j < D; j++) V1[j] = patt(i, j);
dist(i,i) = rtDef;
for (i1 = i + 1; i1 < N; i1++)
{
for (j = 0; j < D; j++) V2[j] = patt(i1, j);
dist(i, i1) = dist(i1, i) = getMetricDistance(V1, V2, metricV, metricKind);
}//for i1
}//for i
calc_dist_pars();
}
int main() {
// refaire les tests de Darray
// Test de Dvector par héritage
Dvector V1(5, 3.0);
std::cout << "V1" << std::endl;
V1.display(std::cout);
Dvector V2(V1);
std::cout << "V2" << std::endl;
V2.display(std::cout);
Dvector V3(5);
std::cout << "V3" << std::endl;
V3.display(std::cout);
Dvector V4;
std::cout << "V4" << std::endl;
V4.display(std::cout);
double d = V1*V3;
std::cout << "d = " << d << std::endl;
}
void ForceConst::Svd(){
int n=Rms.dim1();
int StartLoop=static_cast<int> (Percent*n);
Array2D<double> U, V, S;
Array1D<double> s;
/*
for(int i=0;i<n;i++)
for(int j=0;j<n;j++) {
Rms[i][j]=Rms[i][j]*100.0;
if(Dist[i][j] > 0.6) {
cout << i << " " << j << " " << Dist[i][j] << endl;
Rms[i][j]=0.0;
}
}
*/
SVD<double> G(Rms);
G.getU(U);
G.getV(V);
G.getS(S);
G.getSingularValues(s);
Array2D<double> Sm1(n,n), Ks(n,n), Rms_b(n,n), V1(n,n), U1(n,n), Id(n,n);
for(int i=StartLoop;i<n;i++) S[i][i]=0.0;
V1=transpose(V);
U1=transpose(U);
Rms_b=matmult(U,matmult(S,V1));
Sm1=S;
for(int i=0;i<n;i++)
Sm1[i][i]=(Sm1[i][i] == 0.0)?0.0:1.0/Sm1[i][i];
Ks=matmult(V,matmult(Sm1,U1));
Id=matmult(transpose(Ks),Rms_b);
for(int i=0;i<n;i++) printf(" %5d %12.5e %12.5e \n",i,Rms[i][i],Rms_b[i][i]);
// for(int i=0;i<n;i++)
// for(int j=i;j<n;j++)
// printf(" %5d %5d %12.4f %12.5e %12.5e \n",i,j,Dist[i][j],Ks[i][j], Rms[i][j]);
}
