OpenCascade/occt
1
0
Fork 0
mirror of https://git.dev.opencascade.org/repos/occt.git synced 2025-04-04 18:06:22 +03:00
Code Packages Releases Activity
occt/src/BlendFunc/BlendFunc_ConstRad.cxx
bugmaster 7fd59977df Integration of OCCT 6.5.0 from SVN
2012-03-05 19:23:40 +04:00

1938 lines
55 KiB
C++
Executable File
Raw Blame History

// File: BlendFunc_ConstRad.cxx
// Created: Thu Dec 2 10:18:55 1993
// Author: Jacques GOUSSARD
// Copyright: OPEN CASCADE 1993
// Modified 09/09/1996 PMN Ajout de Nb(Intervalls), IsRationnal
// Optimisation, utilisation de GetCircle
// Modified 20/02/1998 PMN Gestion des surfaces singulieres
#include <BlendFunc_ConstRad.ixx>
#include <math_Gauss.hxx>
#include <math_SVD.hxx>
#include <gp.hxx>
#include <BlendFunc.hxx>
#include <GeomFill.hxx>
#include <Standard_TypeDef.hxx>
#include <Standard_DomainError.hxx>
#include <Standard_NotImplemented.hxx>
#include <ElCLib.hxx>
#include <Precision.hxx>
#define Eps 1.e-15
//=======================================================================
//function : BlendFunc_ConstRad
//purpose :
//=======================================================================
BlendFunc_ConstRad::BlendFunc_ConstRad(const Handle(Adaptor3d_HSurface)& S1,
const Handle(Adaptor3d_HSurface)& S2,
const Handle(Adaptor3d_HCurve)& C)
:
surf1(S1),surf2(S2),
curv(C), tcurv(C),
istangent(Standard_True),
xval(1,4),
E(1,4), DEDX(1,4,1,4), DEDT(1,4),
D2EDX2(4,4,4),
D2EDXDT(1,4,1,4), D2EDT2(1,4),
maxang(RealFirst()), minang(RealLast()),
distmin(RealLast()),
mySShape(BlendFunc_Rational)
{
// Initialisaton des variables de controle du cache.
tval = -9.876e100;
xval.Init(-9.876e100);
myXOrder = -1;
myTOrder = -1;
}
//=======================================================================
//function : NbEquations
//purpose :
//=======================================================================
Standard_Integer BlendFunc_ConstRad::NbEquations () const
{
return 4;
}
//=======================================================================
//function : Set
//purpose :
//=======================================================================
void BlendFunc_ConstRad::Set(const Standard_Real Radius, const Standard_Integer Choix)
{
choix = Choix;
switch (choix) {
case 1:
case 2:
{
ray1 = -Radius;
ray2 = -Radius;
}
break;
case 3:
case 4:
{
ray1 = Radius;
ray2 = -Radius;
}
break;
case 5:
case 6:
{
ray1 = Radius;
ray2 = Radius;
}
break;
case 7:
case 8:
{
ray1 = -Radius;
ray2 = Radius;
}
break;
default:
ray1 = ray2 = -Radius;
}
}
//=======================================================================
//function : Set
//purpose :
//=======================================================================
void BlendFunc_ConstRad::Set(const BlendFunc_SectionShape TypeSection)
{
mySShape = TypeSection;
}
//=======================================================================
//function : ComputeValues
//purpose : Passage OBLIGATOIRE pour tous les calculs
// Cette methode gere les positionemment sur Surfaces et Courbe
// Calcul les equation et leurs derives partielle
// Stock certains resultat intermediaire dans les champs pour etre
// utiliser dans d'autre methodes.
//=======================================================================
Standard_Boolean BlendFunc_ConstRad::ComputeValues(const math_Vector& X,
const Standard_Integer Order,
const Standard_Boolean byParam,
const Standard_Real Param)
{
// declaration statique afin d'eviter la realloc systematique
static gp_Vec d3u1,d3v1,d3uuv1,d3uvv1,d3u2,d3v2,d3uuv2,d3uvv2;
static gp_Vec d1gui, d2gui, d3gui;
static gp_Pnt ptgui;
static Standard_Real invnormtg, dinvnormtg;
Standard_Real T = Param, aux;
// Cas du parametre implicite
if ( !byParam) { T = param;}
// Le travail est il dejas fait ?
Standard_Boolean myX_OK = (Order<=myXOrder) ;
for (Standard_Integer ii=1; ((ii<=X.Length()) && myX_OK); ii++) {
myX_OK = ( X(ii) == xval(ii) );
}
Standard_Boolean t_OK =( (T == tval)
&& ((Order<=myTOrder)||(!byParam)) );
if (myX_OK && (t_OK) ) {
return Standard_True;
}
// Traitement de t
if (!t_OK) {
tval = T;
if (byParam) { myTOrder = Order;}
else { myTOrder = 0;}
//----- Positionement sur la courbe ----------------
switch (myTOrder) {
case 0 :
{
tcurv->D1(T, ptgui, d1gui);
nplan = d1gui.Normalized();
}
break;
case 1 :
{
tcurv->D2(T,ptgui,d1gui,d2gui);
nplan = d1gui.Normalized();
invnormtg = ((Standard_Real) 1 ) / d1gui.Magnitude();
dnplan.SetLinearForm(invnormtg, d2gui,
-invnormtg*(nplan.Dot(d2gui)), nplan);
break;
}
case 2 :
{
tcurv->D3(T,ptgui,d1gui,d2gui,d3gui);
nplan = d1gui.Normalized();
invnormtg = ((Standard_Real) 1 ) / d1gui.Magnitude();
dnplan.SetLinearForm(invnormtg, d2gui,
-invnormtg*(nplan.Dot(d2gui)), nplan);
dinvnormtg = - nplan.Dot(d2gui)*invnormtg*invnormtg;
d2nplan.SetLinearForm(invnormtg, d3gui,
dinvnormtg, d2gui);
aux = dinvnormtg*(nplan.Dot(d2gui)) + invnormtg*( dnplan.Dot(d2gui)
+ nplan.Dot(d3gui) );
d2nplan.SetLinearForm(-invnormtg*(nplan.Dot(d2gui)), dnplan,
-aux, nplan, d2nplan );
break;
}
default:
return Standard_False;
}
}
// Traitement de X
if (!myX_OK) {
xval = X;
myXOrder = Order;
//-------------- Positionement sur les surfaces -----------------
switch (myXOrder) {
case 0 :
{
surf1->D1(X(1),X(2),pts1,d1u1,d1v1);
nsurf1 = d1u1.Crossed(d1v1);
surf2->D1(X(3),X(4),pts2,d1u2,d1v2);
nsurf2 = d1u2.Crossed(d1v2);
break;
}
case 1 :
{
surf1->D2(X(1),X(2),pts1,d1u1,d1v1,d2u1,d2v1,d2uv1);
nsurf1 = d1u1.Crossed(d1v1);
dns1u1 = d2u1.Crossed(d1v1).Added(d1u1.Crossed(d2uv1));
dns1v1 = d2uv1.Crossed(d1v1).Added(d1u1.Crossed(d2v1));
surf2->D2(X(3),X(4),pts2,d1u2,d1v2,d2u2,d2v2,d2uv2);
nsurf2 = d1u2.Crossed(d1v2);
dns1u2 = d2u2.Crossed(d1v2).Added(d1u2.Crossed(d2uv2));
dns1v2 = d2uv2.Crossed(d1v2).Added(d1u2.Crossed(d2v2));
break;
}
case 2 :
{
surf1->D3(X(1),X(2),pts1,d1u1,d1v1,d2u1,d2v1,d2uv1,d3u1,d3v1,d3uuv1,d3uvv1);
nsurf1 = d1u1.Crossed(d1v1);
dns1u1 = d2u1.Crossed(d1v1).Added(d1u1.Crossed(d2uv1));
dns1v1 = d2uv1.Crossed(d1v1).Added(d1u1.Crossed(d2v1));
surf2->D3(X(3),X(4),pts2,d1u2,d1v2,d2u2,d2v2,d2uv2,d3u2,d3v2,d3uuv2,d3uvv2);
nsurf2 = d1u2.Crossed(d1v2);
dns1u2 = d2u2.Crossed(d1v2).Added(d1u2.Crossed(d2uv2));
dns1v2 = d2uv2.Crossed(d1v2).Added(d1u2.Crossed(d2v2));
break;
}
default:
return Standard_False;
}
// Cas des surfaces degeneree
if (nsurf1.Magnitude() < Eps ) {
//gp_Vec normal;
gp_Pnt2d P(X(1), X(2));
if (Order == 0) BlendFunc::ComputeNormal(surf1, P, nsurf1);
else BlendFunc::ComputeDNormal(surf1, P, nsurf1, dns1u1, dns1v1);
}
if (nsurf2.Magnitude() < Eps) {
//gp_Vec normal;
gp_Pnt2d P(X(3), X(4));
if (Order==0) BlendFunc::ComputeNormal(surf2, P, nsurf2);
else BlendFunc::ComputeDNormal(surf2, P, nsurf2, dns1u2, dns1v2);
}
}
// -------------------- Positionement a l'ordre 0 ---------------------
Standard_Real invnorm1, invnorm2, ndotns1, ndotns2, theD;
gp_Vec ncrossns1,ncrossns2,resul,temp;
theD = - (nplan.XYZ().Dot(ptgui.XYZ()));
E(1) = (nplan.X() * (pts1.X() + pts2.X()) +
nplan.Y() * (pts1.Y() + pts2.Y()) +
nplan.Z() * (pts1.Z() + pts2.Z())) /2 + theD;
ncrossns1 = nplan.Crossed(nsurf1);
ncrossns2 = nplan.Crossed(nsurf2);
invnorm1 = ncrossns1.Magnitude();
invnorm2 = ncrossns2.Magnitude();
if (invnorm1 > Eps) invnorm1 = ((Standard_Real) 1) /invnorm1;
else {
invnorm1 = 1; // Insufisant, mais il ne faut pas planter
#if DEB
cout << " ConstRad : Surface singuliere " << endl;
#endif
}
if (invnorm2 > Eps) invnorm2 = ((Standard_Real) 1) /invnorm2;
else {
invnorm2 = 1; // Insufisant, mais il ne faut pas planter
#if DEB
cout << " ConstRad : Surface singuliere " << endl;
#endif
}
ndotns1 = nplan.Dot(nsurf1);
ndotns2 = nplan.Dot(nsurf2);
temp.SetLinearForm(ndotns1,nplan,-1.,nsurf1);
temp.Multiply(invnorm1);
resul.SetLinearForm(ray1,temp,gp_Vec(pts2,pts1));
temp.SetLinearForm(ndotns2,nplan,-1.,nsurf2);
temp.Multiply(invnorm2);
resul.Subtract(ray2*temp);
E(2) = resul.X();
E(3) = resul.Y();
E(4) = resul.Z();
// -------------------- Positionement a l'ordre 1 ---------------------
if (Order >= 1) {
Standard_Real grosterme, cube, carre;
DEDX(1,1) = nplan.Dot(d1u1)/2;
DEDX(1,2) = nplan.Dot(d1v1)/2;
DEDX(1,3) = nplan.Dot(d1u2)/2;
DEDX(1,4) = nplan.Dot(d1v2)/2;
cube =invnorm1*invnorm1*invnorm1;
// Derivee par rapport a u1
grosterme = - ncrossns1.Dot(nplan.Crossed(dns1u1))*cube;
dndu1.SetLinearForm( grosterme*ndotns1
+ invnorm1*nplan.Dot(dns1u1), nplan,
- grosterme, nsurf1,
- invnorm1, dns1u1 );
resul.SetLinearForm(ray1, dndu1, d1u1);
DEDX(2,1) = resul.X();
DEDX(3,1) = resul.Y();
DEDX(4,1) = resul.Z();
// Derivee par rapport a v1
grosterme = - ncrossns1.Dot(nplan.Crossed(dns1v1))*cube;
dndv1.SetLinearForm( grosterme*ndotns1
+invnorm1*nplan.Dot(dns1v1), nplan,
-grosterme, nsurf1,
-invnorm1, dns1v1);
resul.SetLinearForm(ray1, dndv1, d1v1);
DEDX(2,2) = resul.X();
DEDX(3,2) = resul.Y();
DEDX(4,2) = resul.Z();
cube = invnorm2*invnorm2*invnorm2;
// Derivee par rapport a u2
grosterme = - ncrossns2.Dot(nplan.Crossed(dns1u2))*cube;
dndu2.SetLinearForm( grosterme*ndotns2
+invnorm2*nplan.Dot(dns1u2), nplan,
-grosterme, nsurf2,
-invnorm2, dns1u2);
resul.SetLinearForm(-ray2, dndu2, -1, d1u2);
DEDX(2,3) = resul.X();
DEDX(3,3) = resul.Y();
DEDX(4,3) = resul.Z();
// Derivee par rapport a v2
grosterme = -ncrossns2.Dot(nplan.Crossed(dns1v2))*cube;
dndv2.SetLinearForm( grosterme*ndotns2
+invnorm2*nplan.Dot(dns1v2), nplan,
-grosterme, nsurf2,
-invnorm2 , dns1v2);
resul.SetLinearForm(-ray2,dndv2, -1, d1v2);
DEDX(2,4) = resul.X();
DEDX(3,4) = resul.Y();
DEDX(4,4) = resul.Z();
if (byParam) {
temp.SetXYZ( (pts1.XYZ()+pts2.XYZ())/2 - ptgui.XYZ());
// Derivee de n1 par rapport a w
grosterme = ncrossns1.Dot(dnplan.Crossed(nsurf1))*invnorm1*invnorm1;
dn1w.SetLinearForm((dnplan.Dot(nsurf1)-grosterme*ndotns1)*invnorm1, nplan,
ndotns1*invnorm1,dnplan,
grosterme*invnorm1,nsurf1);
// Derivee de n2 par rapport a w
grosterme = ncrossns2.Dot(dnplan.Crossed(nsurf2))*invnorm2*invnorm2;
dn2w.SetLinearForm((dnplan.Dot(nsurf2)-grosterme*ndotns2)*invnorm2,nplan,
ndotns2*invnorm2,dnplan,
grosterme*invnorm2,nsurf2);
DEDT(1) = dnplan.Dot(temp) - 1./invnormtg ;
DEDT(2) = ray1*dn1w.X() - ray2*dn2w.X();
DEDT(3) = ray1*dn1w.Y() - ray2*dn2w.Y();
DEDT(4) = ray1*dn1w.Z() - ray2*dn2w.Z();
}
// ------ Positionemement a l'ordre 2 -----------------------------
if (Order == 2) {
// gp_Vec d2ndu1, d2ndu2, d2ndv1, d2ndv2, d2nduv1, d2nduv2;
gp_Vec d2ns1u1, d2ns1u2, d2ns1v1, d2ns1v2, d2ns1uv1, d2ns1uv2;
Standard_Real uterm, vterm, smallterm, p1, p2, p12;
Standard_Real DPrim, DSecn;
D2EDX2.Init(0);
D2EDX2(1, 1, 1) = nplan.Dot(d2u1)/2;
D2EDX2(1, 2, 1) = D2EDX2(1, 1, 2) = nplan.Dot(d2uv1)/2;
D2EDX2(1, 2, 2) = nplan.Dot(d2v1)/2;
D2EDX2(1, 3, 3) = nplan.Dot(d2u2)/2;
D2EDX2(1, 4, 3) = D2EDX2(1, 3, 4) = nplan.Dot(d2uv2)/2;
D2EDX2(1, 4, 4) = nplan.Dot(d2v2)/2;
// ================
// == Surface 1 ==
// ================
carre = invnorm1*invnorm1;
cube = carre*invnorm1;
// Derivee double par rapport a u1
// Derivation de la norme
d2ns1u1.SetLinearForm(1, d3u1.Crossed(d1v1),
2, d2u1.Crossed(d2uv1),
1, d1u1.Crossed(d3uuv1));
DPrim = ncrossns1.Dot(nplan.Crossed(dns1u1));
smallterm = - 2*DPrim*cube;
DSecn = ncrossns1.Dot(nplan.Crossed(d2ns1u1))
+ (nplan.Crossed(dns1u1)).SquareMagnitude();
grosterme = (3*DPrim*DPrim*carre - DSecn) * cube;
temp.SetLinearForm( grosterme*ndotns1, nplan,
-grosterme , nsurf1);
p1 = nplan.Dot(dns1u1);
p2 = nplan.Dot(d2ns1u1);
d2ndu1.SetLinearForm( invnorm1*p2
+smallterm*p1, nplan,
-smallterm, dns1u1,
-invnorm1, d2ns1u1);
d2ndu1 += temp;
resul.SetLinearForm(ray1, d2ndu1, d2u1);
D2EDX2(2,1,1) = resul.X();
D2EDX2(3,1,1) = resul.Y();
D2EDX2(4,1,1) = resul.Z();
// Derivee double par rapport a u1, v1
// Derivation de la norme
d2ns1uv1 = (d3uuv1.Crossed(d1v1))
+ (d2u1 .Crossed(d2v1))
+ (d1u1 .Crossed(d3uvv1));
uterm = ncrossns1.Dot(nplan.Crossed(dns1u1));
vterm = ncrossns1.Dot(nplan.Crossed(dns1v1));
DSecn = (nplan.Crossed(dns1v1)).Dot(nplan.Crossed(dns1u1))
+ ncrossns1.Dot(nplan.Crossed(d2ns1uv1));
grosterme = (3*uterm*vterm*carre-DSecn)*cube;
uterm *= -cube; //et seulement maintenant
vterm *= -cube;
p1 = nplan.Dot(dns1u1);
p2 = nplan.Dot(dns1v1);
temp.SetLinearForm( grosterme*ndotns1, nplan,
- grosterme, nsurf1,
- invnorm1, d2ns1uv1);
d2nduv1.SetLinearForm( invnorm1*nplan.Dot(d2ns1uv1)
+ uterm*p2
+ vterm*p1, nplan,
- uterm, dns1v1,
- vterm, dns1u1);
d2nduv1 += temp;
resul.SetLinearForm(ray1, d2nduv1, d2uv1);
D2EDX2(2,2,1) = D2EDX2(2,1,2) = resul.X();
D2EDX2(3,2,1) = D2EDX2(3,1,2) = resul.Y();
D2EDX2(4,2,1) = D2EDX2(4,1,2) = resul.Z();
// Derivee double par rapport a v1
// Derivation de la norme
d2ns1v1.SetLinearForm(1, d1u1.Crossed(d3v1),
2, d2uv1.Crossed(d2v1),
1, d3uvv1.Crossed(d1v1));
DPrim = ncrossns1.Dot(nplan.Crossed(dns1v1));
smallterm = - 2*DPrim*cube;
DSecn = ncrossns1.Dot(nplan.Crossed(d2ns1v1))
+ (nplan.Crossed(dns1v1)).SquareMagnitude();
grosterme = (3*DPrim*DPrim*carre - DSecn) * cube;
p1 = nplan.Dot(dns1v1);
p2 = nplan.Dot(d2ns1v1);
temp.SetLinearForm( grosterme*ndotns1, nplan,
-grosterme , nsurf1);
d2ndv1.SetLinearForm( invnorm1*p2
+smallterm*p1, nplan,
-smallterm, dns1v1,
-invnorm1, d2ns1v1);
d2ndv1 += temp;
resul.SetLinearForm(ray1, d2ndv1, d2v1);
D2EDX2(2,2,2) = resul.X();
D2EDX2(3,2,2) = resul.Y();
D2EDX2(4,2,2) = resul.Z();
// ================
// == Surface 2 ==
// ================
carre = invnorm2*invnorm2;
cube = carre*invnorm2;
// Derivee double par rapport a u2
// Derivation de la norme
d2ns1u2.SetLinearForm(1, d3u2.Crossed(d1v2),
2, d2u2.Crossed(d2uv2),
1, d1u2.Crossed(d3uuv2));
DPrim = ncrossns2.Dot(nplan.Crossed(dns1u2));
smallterm = - 2*DPrim*cube;
DSecn = ncrossns2.Dot(nplan.Crossed(d2ns1u2))
+ (nplan.Crossed(dns1u2)).SquareMagnitude();
grosterme = (3*DPrim*DPrim*carre - DSecn) * cube;
temp.SetLinearForm( grosterme*ndotns2, nplan,
-grosterme , nsurf2);
p1 = nplan.Dot(dns1u2);
p2 = nplan.Dot(d2ns1u2);
d2ndu2.SetLinearForm( invnorm2*p2
+smallterm*p1, nplan,
-smallterm, dns1u2,
-invnorm2, d2ns1u2);
d2ndu2 += temp;
resul.SetLinearForm(-ray2, d2ndu2, -1, d2u2);
D2EDX2(2,3,3) = resul.X();
D2EDX2(3,3,3) = resul.Y();
D2EDX2(4,3,3) = resul.Z();
// Derivee double par rapport a u2, v2
// Derivation de la norme
d2ns1uv2 = (d3uuv2.Crossed(d1v2))
+ (d2u2 .Crossed(d2v2))
+ (d1u2 .Crossed(d3uvv2));
uterm = ncrossns2.Dot(nplan.Crossed(dns1u2));
vterm = ncrossns2.Dot(nplan.Crossed(dns1v2));
DSecn = (nplan.Crossed(dns1v2)).Dot(nplan.Crossed(dns1u2))
+ ncrossns2.Dot(nplan.Crossed(d2ns1uv2));
grosterme = (3*uterm*vterm*carre-DSecn)*cube;
uterm *= -cube; //et seulement maintenant
vterm *= -cube;
p1 = nplan.Dot(dns1u2);
p2 = nplan.Dot(dns1v2);
temp.SetLinearForm( grosterme*ndotns2, nplan,
- grosterme, nsurf2,
- invnorm2, d2ns1uv2);
d2nduv2.SetLinearForm( invnorm2*nplan.Dot(d2ns1uv2)
+ uterm*p2
+ vterm*p1, nplan,
- uterm, dns1v2,
- vterm, dns1u2);
d2nduv2 += temp;
resul.SetLinearForm(-ray2, d2nduv2, -1, d2uv2);
D2EDX2(2,4,3) = D2EDX2(2,3,4) = resul.X();
D2EDX2(3,4,3) = D2EDX2(3,3,4) = resul.Y();
D2EDX2(4,4,3) = D2EDX2(4,3,4) = resul.Z();
// Derivee double par rapport a v2
// Derivation de la norme
d2ns1v2.SetLinearForm(1, d1u2.Crossed(d3v2),
2, d2uv2.Crossed(d2v2),
1, d3uvv2.Crossed(d1v2));
DPrim = ncrossns2.Dot(nplan.Crossed(dns1v2));
smallterm = - 2*DPrim*cube;
DSecn = ncrossns2.Dot(nplan.Crossed(d2ns1v2))
+ (nplan.Crossed(dns1v2)).SquareMagnitude();
grosterme = (3*DPrim*DPrim*carre - DSecn) * cube;
p1 = nplan.Dot(dns1v2);
p2 = nplan.Dot(d2ns1v2);
temp.SetLinearForm( grosterme*ndotns2, nplan,
-grosterme , nsurf2);
d2ndv2.SetLinearForm( invnorm2*p2
+smallterm*p1, nplan,
-smallterm, dns1v2,
-invnorm2, d2ns1v2);
d2ndv2 += temp;
resul.SetLinearForm(-ray2, d2ndv2, -1, d2v2);
D2EDX2(2,4,4) = resul.X();
D2EDX2(3,4,4) = resul.Y();
D2EDX2(4,4,4) = resul.Z();
if (byParam) {
Standard_Real tterm;
// ---------- Derivation double en t, X --------------------------
D2EDXDT(1,1) = dnplan.Dot(d1u1)/2;
D2EDXDT(1,2) = dnplan.Dot(d1v1)/2;
D2EDXDT(1,3) = dnplan.Dot(d1u2)/2;
D2EDXDT(1,4) = dnplan.Dot(d1v2)/2;
carre = invnorm1*invnorm1;
cube = carre*invnorm1;
//--> Derivee par rapport a u1 et t
tterm = ncrossns1.Dot(dnplan.Crossed(nsurf1));
smallterm = - tterm*cube;
// Derivation de la norme
uterm = ncrossns1.Dot(nplan. Crossed(dns1u1));
DSecn = (nplan.Crossed(dns1u1)).Dot(dnplan.Crossed(nsurf1))
+ ncrossns1.Dot(dnplan.Crossed(dns1u1));
grosterme = (3*uterm*tterm*carre - DSecn) * cube;
uterm *= -cube;
p1 = dnplan.Dot(nsurf1);
p2 = nplan. Dot(dns1u1);
p12 = dnplan.Dot(dns1u1);
d2ndtu1.SetLinearForm( invnorm1*p12
+ smallterm*p2
+ uterm*p1
+ grosterme*ndotns1, nplan,
invnorm1*p2
+ uterm*ndotns1, dnplan,
- smallterm, dns1u1);
d2ndtu1 -= grosterme*nsurf1;
resul = ray1*d2ndtu1;
D2EDXDT(2,1) = resul.X();
D2EDXDT(3,1) = resul.Y();
D2EDXDT(4,1) = resul.Z();
//--> Derivee par rapport a v1 et t
// Derivation de la norme
uterm = ncrossns1.Dot(nplan. Crossed(dns1v1));
DSecn = (nplan. Crossed(dns1v1)).Dot(dnplan.Crossed(nsurf1))
+ ncrossns1.Dot(dnplan.Crossed(dns1v1));
grosterme = (3*uterm*tterm*carre - DSecn) * cube;
uterm *= -cube;
p1 = dnplan.Dot(nsurf1);
p2 = nplan. Dot(dns1v1);
p12 = dnplan.Dot(dns1v1);
d2ndtv1.SetLinearForm( invnorm1*p12
+ uterm*p1
+ smallterm*p2
+ grosterme*ndotns1, nplan,
invnorm1*p2
+ uterm*ndotns1, dnplan,
- smallterm , dns1v1);
d2ndtv1 -= grosterme*nsurf1;
resul = ray1* d2ndtv1;
D2EDXDT(2,2) = resul.X();
D2EDXDT(3,2) = resul.Y();
D2EDXDT(4,2) = resul.Z();
carre = invnorm2*invnorm2;
cube = carre*invnorm2;
//--> Derivee par rapport a u2 et t
tterm = ncrossns2.Dot(dnplan.Crossed(nsurf2));
smallterm = -tterm*cube;
// Derivation de la norme
uterm = ncrossns2.Dot(nplan. Crossed(dns1u2));
DSecn = (nplan. Crossed(dns1u2)).Dot(dnplan.Crossed(nsurf2))
+ ncrossns2.Dot(dnplan.Crossed(dns1u2));
grosterme = (3*uterm*tterm*carre - DSecn) * cube;
uterm *= -cube;
p1 = dnplan.Dot(nsurf2);
p2 = nplan. Dot(dns1u2);
p12 = dnplan.Dot(dns1u2);
d2ndtu2.SetLinearForm( invnorm2*p12
+ smallterm*p2
+ uterm*p1
+ grosterme*ndotns2, nplan,
invnorm2*p2
+ uterm*ndotns2, dnplan,
-smallterm , dns1u2);
d2ndtu2 -= grosterme*nsurf2;
resul = - ray2*d2ndtu2;
D2EDXDT(2,3) = resul.X();
D2EDXDT(3,3) = resul.Y();
D2EDXDT(4,3) = resul.Z();
//--> Derivee par rapport a v2 et t
// Derivation de la norme
uterm = ncrossns2.Dot(nplan. Crossed(dns1v2));
DSecn = (nplan.Crossed(dns1v2)).Dot(dnplan.Crossed(nsurf2))
+ ncrossns2.Dot(dnplan.Crossed(dns1v2));
grosterme = (3*uterm*tterm*carre - DSecn) * cube;
uterm *= - cube;
p1 = dnplan.Dot(nsurf2);
p2 = nplan. Dot(dns1v2);
p12 = dnplan.Dot(dns1v2);
d2ndtv2.SetLinearForm( invnorm2*p12
+ smallterm*p2
+ uterm*p1
+ grosterme*ndotns2, nplan,
invnorm2*p2
+ uterm*ndotns2, dnplan,
-smallterm , dns1v2);
d2ndtv2 -= grosterme*nsurf2;
resul = - ray2 * d2ndtv2;
D2EDXDT(2,4) = resul.X();
D2EDXDT(3,4) = resul.Y();
D2EDXDT(4,4) = resul.Z();
// ---------- Derivation double en t -----------------------------
// Derivee de n1 par rapport a w
carre = invnorm1*invnorm1;
cube = carre*invnorm1;
// Derivation de la norme
DPrim = ncrossns1.Dot(dnplan.Crossed(nsurf1));
smallterm = - 2*DPrim*cube;
DSecn = (dnplan.Crossed(nsurf1)).SquareMagnitude()
+ ncrossns1.Dot(d2nplan.Crossed(nsurf1));
grosterme = (3*DPrim*DPrim*carre - DSecn) * cube;
p1 = dnplan. Dot(nsurf1);
p2 = d2nplan.Dot(nsurf1);
temp.SetLinearForm( grosterme*ndotns1, nplan,
-grosterme , nsurf1);
d2n1w.SetLinearForm( smallterm*p1
+ invnorm1*p2, nplan,
smallterm*ndotns1
+ 2*invnorm1*p1, dnplan,
ndotns1*invnorm1, d2nplan);
d2n1w += temp;
// Derivee de n2 par rapport a w
carre = invnorm2*invnorm2;
cube = carre*invnorm2;
// Derivation de la norme
DPrim = ncrossns2.Dot(dnplan.Crossed(nsurf2));
smallterm = - 2*DPrim*cube;
DSecn = (dnplan.Crossed(nsurf2)).SquareMagnitude()
+ ncrossns2.Dot(d2nplan.Crossed(nsurf2));
grosterme = (3*DPrim*DPrim*carre - DSecn) * cube;
p1 = dnplan. Dot(nsurf2);
p2 = d2nplan.Dot(nsurf2);
temp.SetLinearForm( grosterme*ndotns2, nplan,
-grosterme , nsurf2);
d2n2w.SetLinearForm( smallterm*p1
+ invnorm2*p2, nplan,
smallterm*ndotns2
+ 2*invnorm2*p1, dnplan,
ndotns2*invnorm2, d2nplan);
d2n2w += temp;
temp.SetXYZ( (pts1.XYZ()+pts2.XYZ())/2 - ptgui.XYZ());
D2EDT2(1) = d2nplan.Dot(temp) - 2*dnplan.Dot(d1gui) - nplan.Dot(d2gui);
D2EDT2(2) = ray1*d2n1w.X() - ray2*d2n2w.X();
D2EDT2(3) = ray1*d2n1w.Y() - ray2*d2n2w.Y();
D2EDT2(4) = ray1*d2n1w.Z() - ray2*d2n2w.Z();
}
}
}
return Standard_True;
}
//=======================================================================
//function : Set
//purpose :
//=======================================================================
void BlendFunc_ConstRad::Set(const Standard_Real Param)
{
param = Param;
}
//=======================================================================
//function : Set
//purpose : Segmente la courbe a sa partie utile.
// La prcision est prise arbitrairement petite !?
//=======================================================================
void BlendFunc_ConstRad::Set(const Standard_Real First, const Standard_Real Last)
{
tcurv = curv->Trim(First, Last, 1.e-12);
}
//=======================================================================
//function : GetTolerance
//purpose :
//=======================================================================
void BlendFunc_ConstRad::GetTolerance(math_Vector& Tolerance, const Standard_Real Tol) const
{
Tolerance(1) = surf1->UResolution(Tol);
Tolerance(2) = surf1->VResolution(Tol);
Tolerance(3) = surf2->UResolution(Tol);
Tolerance(4) = surf2->VResolution(Tol);
}
//=======================================================================
//function : GetBounds
//purpose :
//=======================================================================
void BlendFunc_ConstRad::GetBounds(math_Vector& InfBound, math_Vector& SupBound) const
{
InfBound(1) = surf1->FirstUParameter();
InfBound(2) = surf1->FirstVParameter();
InfBound(3) = surf2->FirstUParameter();
InfBound(4) = surf2->FirstVParameter();
SupBound(1) = surf1->LastUParameter();
SupBound(2) = surf1->LastVParameter();
SupBound(3) = surf2->LastUParameter();
SupBound(4) = surf2->LastVParameter();
for(Standard_Integer i = 1; i <= 4; i++){
if(!Precision::IsInfinite(InfBound(i)) &&
!Precision::IsInfinite(SupBound(i))) {
Standard_Real range = (SupBound(i) - InfBound(i));
InfBound(i) -= range;
SupBound(i) += range;
}
}
}
//=======================================================================
//function : IsSolution
//purpose :
//=======================================================================
Standard_Boolean BlendFunc_ConstRad::IsSolution(const math_Vector& Sol, const Standard_Real Tol)
{
Standard_Real norm, Cosa, Sina, Angle;
Standard_Boolean Ok=Standard_True;
Ok = ComputeValues(Sol, 1, Standard_True, param);
if (Abs(E(1)) <= Tol &&
E(2)*E(2) + E(3)*E(3) + E(4)*E(4) <= Tol*Tol) {
// on recopie localement ns1, ns2, np afin de ne pas ecraser les champs !
gp_Vec ns1,ns2,np;
ns1 = nsurf1;
ns2 = nsurf2;
np = nplan;
norm = nplan.Crossed(ns1).Magnitude();
if (norm < Eps) {
norm = 1; // Insufisant, mais il ne faut pas planter
}
ns1.SetLinearForm(nplan.Dot(ns1)/norm,nplan, -1./norm, ns1);
norm = nplan.Crossed(ns2).Magnitude();
if (norm < Eps) {
norm = 1; // Insufisant, mais il ne faut pas planter
}
ns2.SetLinearForm(nplan.Dot(ns2)/norm,nplan, -1./norm, ns2);
Standard_Real maxpiv = 1.e-9;
math_Vector controle(1,4),solution(1,4), tolerances(1,4);
GetTolerance(tolerances,Tol);
istangent = Standard_True;
math_Gauss Resol(DEDX,maxpiv);
if (Resol.IsDone()) {
Resol.Solve(-DEDT,solution);
istangent = Standard_False;
controle = DEDT.Added(DEDX.Multiplied(solution));
if(Abs(controle(1)) > tolerances(1) ||
Abs(controle(2)) > tolerances(2) ||
Abs(controle(3)) > tolerances(3) ||
Abs(controle(4)) > tolerances(4)){
istangent = Standard_True;
}
}
if (istangent) {
math_SVD SingRS (DEDX);
if (SingRS.IsDone()) {
SingRS.Solve(-DEDT, solution, 1.e-6);
istangent = Standard_False;
controle = DEDT.Added(DEDX.Multiplied(solution));
if(Abs(controle(1)) > tolerances(1) ||
Abs(controle(2)) > tolerances(2) ||
Abs(controle(3)) > tolerances(3) ||
Abs(controle(4)) > tolerances(4)){
#ifdef DEB
cout<<"Cheminement : echec calcul des derivees"<<endl;
#endif
istangent = Standard_True;
}
}
}
if(!istangent){
tg1.SetLinearForm(solution(1),d1u1,solution(2),d1v1);
tg2.SetLinearForm(solution(3),d1u2,solution(4),d1v2);
tg12d.SetCoord(solution(1),solution(2));
tg22d.SetCoord(solution(3),solution(4));
}
// mise a jour de maxang
if (ray1 > 0.) {
ns1.Reverse();
}
if (ray2 >0.) {
ns2.Reverse();
}
Cosa = ns1.Dot(ns2);
Sina = np.Dot(ns1.Crossed(ns2));
if (choix%2 != 0) {
Sina = -Sina; //nplan est change en -nplan
}
if(Cosa > 1.) {Cosa = 1.; Sina = 0.;}
Angle = ACos(Cosa);
// Recadrage sur ]-pi/2, 3pi/2]
if (Sina <0.) {
if (Cosa > 0.) Angle = -Angle;
else Angle = 2.*PI - Angle;
}
// cout << "Angle : " <<Angle << endl;
// if ((Angle < 0) || (Angle > PI)) {
// cout << "t = " << param << endl;
// }
if (Abs(Angle)>maxang) {maxang = Abs(Angle);}
if (Abs(Angle)<minang) {minang = Abs(Angle);}
distmin = Min( distmin, pts1.Distance(pts2));
return Ok;
}
istangent = Standard_True;
return Standard_False;
}
//=======================================================================
//function : GetMinimalDistance
//purpose :
//=======================================================================
Standard_Real BlendFunc_ConstRad::GetMinimalDistance() const
{
return distmin;
}
//=======================================================================
//function : Value
//purpose :
//=======================================================================
Standard_Boolean BlendFunc_ConstRad::Value(const math_Vector& X, math_Vector& F)
{
const Standard_Boolean Ok = ComputeValues(X, 0);
F = E;
return Ok;
}
//=======================================================================
//function : Derivatives
//purpose :
//=======================================================================
Standard_Boolean BlendFunc_ConstRad::Derivatives(const math_Vector& X, math_Matrix& D)
{
const Standard_Boolean Ok = ComputeValues(X, 1);
D = DEDX;
return Ok;
}
//=======================================================================
//function : Values
//purpose :
//=======================================================================
Standard_Boolean BlendFunc_ConstRad::Values(const math_Vector& X, math_Vector& F, math_Matrix& D)
{
const Standard_Boolean Ok = ComputeValues(X, 1);
F = E;
D = DEDX;
return Ok;
}
//=======================================================================
//function : PointOnS1
//purpose :
//=======================================================================
const gp_Pnt& BlendFunc_ConstRad::PointOnS1 () const
{
return pts1;
}
//=======================================================================
//function : PointOnS2
//purpose :
//=======================================================================
const gp_Pnt& BlendFunc_ConstRad::PointOnS2 () const
{
return pts2;
}
//=======================================================================
//function : IsTangencyPoint
//purpose :
//=======================================================================
Standard_Boolean BlendFunc_ConstRad::IsTangencyPoint () const
{
return istangent;
}
//=======================================================================
//function : TangentOnS1
//purpose :
//=======================================================================
const gp_Vec& BlendFunc_ConstRad::TangentOnS1 () const
{
if (istangent)
Standard_DomainError::Raise("BlendFunc_ConstRad::TangentOnS1");
return tg1;
}
//=======================================================================
//function : TangentOnS2
//purpose :
//=======================================================================
const gp_Vec& BlendFunc_ConstRad::TangentOnS2 () const
{
if (istangent)
Standard_DomainError::Raise("BlendFunc_ConstRad::TangentOnS2");
return tg2;
}
//=======================================================================
//function : Tangent2dOnS1
//purpose :
//=======================================================================
const gp_Vec2d& BlendFunc_ConstRad::Tangent2dOnS1 () const
{
if (istangent)
Standard_DomainError::Raise("BlendFunc_ConstRad::Tangent2dOnS1");
return tg12d;
}
//=======================================================================
//function : Tangent2dOnS2
//purpose :
//=======================================================================
const gp_Vec2d& BlendFunc_ConstRad::Tangent2dOnS2 () const
{
if (istangent)
Standard_DomainError::Raise("BlendFunc_ConstRad::Tangent2dOnS2");
return tg22d;
}
//=======================================================================
//function : Tangent
//purpose :
//=======================================================================
void BlendFunc_ConstRad::Tangent(const Standard_Real U1,
const Standard_Real V1,
const Standard_Real U2,
const Standard_Real V2,
gp_Vec& TgF,
gp_Vec& TgL,
gp_Vec& NmF,
gp_Vec& NmL) const
{
gp_Pnt Center;
gp_Vec ns1;
Standard_Real invnorm1;
if ((U1!=xval(1)) || (V1!=xval(2)) ||
(U2!=xval(3)) || (V2!=xval(4))) {
gp_Vec d1u,d1v;
gp_Pnt bid;
//#if DEB
// cout << " ConstRad::erreur de tengent !!!!!!!!!!!!!!!!!!!!" << endl;
//#endif
surf1->D1(U1,V1,bid,d1u,d1v);
NmF = ns1 = d1u.Crossed(d1v);
surf2->D1(U2,V2,bid,d1u,d1v);
NmL = d1u.Crossed(d1v);
}
else {
NmF = ns1 = nsurf1;
NmL = nsurf2;
}
invnorm1 = nplan.Crossed(ns1).Magnitude();
if (invnorm1 < Eps) invnorm1 = 1;
else invnorm1 = 1. / invnorm1;
ns1.SetLinearForm(nplan.Dot(ns1)*invnorm1,nplan, -invnorm1,ns1);
Center.SetXYZ(pts1.XYZ()+ray1*ns1.XYZ());
TgF = nplan.Crossed(gp_Vec(Center,pts1));
TgL = nplan.Crossed(gp_Vec(Center,pts2));
if (choix%2 == 1) {
TgF.Reverse();
TgL.Reverse();
}
}
//=======================================================================
//function : TwistOnS1
//purpose :
//=======================================================================
Standard_Boolean BlendFunc_ConstRad::TwistOnS1() const
{
if (istangent)
Standard_DomainError::Raise("BlendFunc_ConstRad::TwistOnS1");
return tg1.Dot(nplan) < 0.;
}
//=======================================================================
//function : TwistOnS2
//purpose :
//=======================================================================
Standard_Boolean BlendFunc_ConstRad::TwistOnS2() const
{
if (istangent)
Standard_DomainError::Raise("BlendFunc_ConstRad::TwistOnS2");
return tg2.Dot(nplan) < 0.;
}
//=======================================================================
//function : Section
//purpose :
//=======================================================================
void BlendFunc_ConstRad::Section(const Standard_Real Param,
const Standard_Real U1,
const Standard_Real V1,
const Standard_Real U2,
const Standard_Real V2,
Standard_Real& Pdeb,
Standard_Real& Pfin,
gp_Circ& C)
{
gp_Pnt Center;
gp_Vec ns1,np;
math_Vector X(1,4);
X(1) = U1; X(2) = V1; X(3) = U2; X(4) = V2;
Standard_Real prm = Param;
Standard_Boolean Ok;
Ok = ComputeValues(X, 0, Standard_True, prm);
ns1 = nsurf1;
np = nplan;
Standard_Real norm1;
norm1 = nplan.Crossed(ns1).Magnitude();
if (norm1 < Eps) {
norm1 = 1; // Insufisant, mais il ne faut pas planter
}
ns1.SetLinearForm(nplan.Dot(ns1)/norm1,nplan, -1./norm1,ns1);
Center.SetXYZ(pts1.XYZ()+ray1*ns1.XYZ());
// on oriente ns1 du centre vers pts1,
if (ray1 > 0.) {
ns1.Reverse();
}
if (choix%2 != 0) {
np.Reverse();
}
C.SetRadius(Abs(ray1));
C.SetPosition(gp_Ax2(Center,np,ns1));
Pdeb = 0.;
Pfin = ElCLib::Parameter(C,pts2);
// Test des angles negatif et quasi null : Cas Singulier
if (Pfin>1.5*PI) {
np.Reverse();
C.SetPosition(gp_Ax2(Center,np,ns1));
Pfin = ElCLib::Parameter(C,pts2);
}
if (Pfin < Precision::PConfusion()) Pfin += Precision::PConfusion();
}
//=======================================================================
//function : IsRational
//purpose :
//=======================================================================
Standard_Boolean BlendFunc_ConstRad::IsRational () const
{
return (mySShape==BlendFunc_Rational || mySShape==BlendFunc_QuasiAngular);
}
//=======================================================================
//function : GetSectionSize
//purpose :
//=======================================================================
Standard_Real BlendFunc_ConstRad::GetSectionSize() const
{
return maxang*Abs(ray1);
}
//=======================================================================
//function : GetMinimalWeight
//purpose :
//=======================================================================
void BlendFunc_ConstRad::GetMinimalWeight(TColStd_Array1OfReal& Weigths) const
{
BlendFunc::GetMinimalWeights(mySShape, myTConv, minang, maxang, Weigths );
// On suppose que cela ne depend pas du Rayon!
}
//=======================================================================
//function : NbIntervals
//purpose :
//=======================================================================
Standard_Integer BlendFunc_ConstRad::NbIntervals (const GeomAbs_Shape S) const
{
return curv->NbIntervals(BlendFunc::NextShape(S));
}
//=======================================================================
//function : Intervals
//purpose :
//=======================================================================
void BlendFunc_ConstRad::Intervals (TColStd_Array1OfReal& T,
const GeomAbs_Shape S) const
{
curv->Intervals(T, BlendFunc::NextShape(S));
}
//=======================================================================
//function : GetShape
//purpose :
//=======================================================================
void BlendFunc_ConstRad::GetShape (Standard_Integer& NbPoles,
Standard_Integer& NbKnots,
Standard_Integer& Degree,
Standard_Integer& NbPoles2d)
{
NbPoles2d = 2;
BlendFunc::GetShape(mySShape,maxang,NbPoles,NbKnots,Degree,myTConv);
}
//=======================================================================
//function : GetTolerance
//purpose : Determine les Tolerances a utiliser dans les approximations.
//=======================================================================
void BlendFunc_ConstRad::GetTolerance(const Standard_Real BoundTol,
const Standard_Real SurfTol,
const Standard_Real AngleTol,
math_Vector& Tol3d,
math_Vector& Tol1d) const
{
Standard_Integer low = Tol3d.Lower() , up=Tol3d.Upper();
Standard_Real Tol;
Tol= GeomFill::GetTolerance(myTConv, minang, Abs(ray1),
AngleTol, SurfTol);
Tol1d.Init(SurfTol);
Tol3d.Init(SurfTol);
Tol3d(low+1) = Tol3d(up-1) = Min(Tol, SurfTol);
Tol3d(low) = Tol3d(up) = Min(Tol, BoundTol);
}
//=======================================================================
//function : Knots
//purpose :
//=======================================================================
void BlendFunc_ConstRad::Knots(TColStd_Array1OfReal& TKnots)
{
GeomFill::Knots(myTConv,TKnots);
}
//=======================================================================
//function : Mults
//purpose :
//=======================================================================
void BlendFunc_ConstRad::Mults(TColStd_Array1OfInteger& TMults)
{
GeomFill::Mults(myTConv,TMults);
}
//=======================================================================
//function : Section
//purpose :
//=======================================================================
void BlendFunc_ConstRad::Section(const Blend_Point& P,
TColgp_Array1OfPnt& Poles,
TColgp_Array1OfPnt2d& Poles2d,
TColStd_Array1OfReal& Weights)
{
gp_Pnt Center;
gp_Vec ns1,ns2,np;
math_Vector X(1,4);
Standard_Real prm = P.Parameter();
Standard_Boolean Ok;
Standard_Integer low = Poles.Lower();
Standard_Integer upp = Poles.Upper();
P.ParametersOnS1(X(1), X(2));
P.ParametersOnS2(X(3), X(4));
Ok = ComputeValues(X, 0, Standard_True, prm);
distmin = Min (distmin, pts1.Distance(pts2));
// on recopie localement ns1, ns2, np afin de ne pas ecraser les champs !
ns1 = nsurf1;
ns2 = nsurf2;
np = nplan;
Poles2d(Poles2d.Lower()).SetCoord(X(1), X(2));
Poles2d(Poles2d.Upper()).SetCoord(X(3), X(4));
if (mySShape == BlendFunc_Linear) {
Poles(low) = pts1;
Poles(upp) = pts2;
Weights(low) = 1.0;
Weights(upp) = 1.0;
return;
}
Standard_Real norm1, norm2;
norm1 = nplan.Crossed(ns1).Magnitude();
norm2 = nplan.Crossed(ns2).Magnitude();
if (norm1 < Eps) {
norm1 = 1; // Insufisant, mais il ne faut pas planter
//#if DEB
// cout << " ConstRad : Surface singuliere " << endl;
//#endif
}
if (norm2 < Eps) {
norm2 = 1; // Insufisant, mais il ne faut pas planter
//#if DEB
// cout << " ConstRad : Surface singuliere " << endl;
//#endif
}
ns1.SetLinearForm(nplan.Dot(ns1)/norm1,nplan, -1./norm1,ns1);
ns2.SetLinearForm(nplan.Dot(ns2)/norm2,nplan, -1./norm2,ns2);
Center.SetXYZ(pts1.XYZ()+ray1*ns1.XYZ());
// on oriente ns1 (resp. ns2) du centre vers pts1 (resp. pts2),
// et on rend direct le triedre ns1,ns2,nplan.
if (ray1 > 0.) {
ns1.Reverse();
}
if (ray2 >0.) {
ns2.Reverse();
}
if (choix%2 != 0) {
np.Reverse();
}
GeomFill::GetCircle(myTConv,
ns1, ns2,
np, pts1, pts2,
Abs(ray1), Center,
Poles, Weights);
}
//=======================================================================
//function : Section
//purpose :
//=======================================================================
Standard_Boolean BlendFunc_ConstRad::Section
(const Blend_Point& P,
TColgp_Array1OfPnt& Poles,
TColgp_Array1OfVec& DPoles,
TColgp_Array1OfPnt2d& Poles2d,
TColgp_Array1OfVec2d& DPoles2d,
TColStd_Array1OfReal& Weights,
TColStd_Array1OfReal& DWeights)
{
gp_Vec ns1, ns2, np, dnp, dnorm1w, dnorm2w, tgc;
Standard_Real norm1, norm2;
gp_Pnt Center;
math_Vector sol(1,4), secmember(1,4);
Standard_Real prm = P.Parameter();
Standard_Integer low = Poles.Lower();
Standard_Integer upp = Poles.Upper();
Standard_Boolean istgt = Standard_True;
P.ParametersOnS1(sol(1),sol(2));
P.ParametersOnS2(sol(3),sol(4));
// Calculs des equations
ComputeValues(sol, 1, Standard_True, prm);
distmin = Min (distmin, pts1.Distance(pts2));
// on recopie localement ns1, ns2, np afin de ne pas ecraser les champs !
ns1 = nsurf1;
ns2 = nsurf2;
np = nplan;
dnp = dnplan;
if ( ! pts1.IsEqual(pts2, 1.e-4)) {
// Calcul des derives Traitement Normal
math_Gauss Resol(DEDX, 1.e-9);
if (Resol.IsDone()) {
Resol.Solve(-DEDT, secmember);
istgt = Standard_False;
}
}
if (istgt) {
math_SVD SingRS (DEDX);
if (SingRS.IsDone()) {
SingRS.Solve(-DEDT, secmember, 1.e-6);
istgt = Standard_False;
}
}
if (!istgt) {
tg1.SetLinearForm(secmember(1),d1u1, secmember(2),d1v1);
tg2.SetLinearForm(secmember(3),d1u2, secmember(4),d1v2);
dnorm1w.SetLinearForm(secmember(1),dndu1, secmember(2),dndv1, dn1w);
dnorm2w.SetLinearForm(secmember(3),dndu2, secmember(4),dndv2, dn2w);
}
// Les poles 2d
Poles2d(Poles2d.Lower()).SetCoord(sol(1),sol(2));
Poles2d(Poles2d.Upper()).SetCoord(sol(3),sol(4));
if (!istgt) {
DPoles2d(Poles2d.Lower()).SetCoord(secmember(1),secmember(2));
DPoles2d(Poles2d.Upper()).SetCoord(secmember(3),secmember(4));
}
// on traite le cas linear...
if (mySShape == BlendFunc_Linear) {
Poles(low) = pts1;
Poles(upp) = pts2;
Weights(low) = 1.0;
Weights(upp) = 1.0;
if (!istgt) {
DPoles(low) = tg1;
DPoles(upp) = tg2;
DWeights(low) = 0.0;
DWeights(upp) = 0.0;
}
return (!istgt);
}
// Cas du cercle
norm1 = nplan.Crossed(ns1).Magnitude();
norm2 = nplan.Crossed(ns2).Magnitude();
if (norm1 < Eps) {
norm1 = 1; // Insufisant, mais il ne faut pas planter
#if DEB
cout << " ConstRad : Surface singuliere " << endl;
#endif
}
if (norm2 < Eps) {
norm2 = 1; // Insufisant, mais il ne faut pas planter
#if DEB
cout << " ConstRad : Surface singuliere " << endl;
#endif
}
ns1.SetLinearForm(nplan.Dot(ns1)/norm1,nplan, -1./norm1,ns1);
ns2.SetLinearForm(nplan.Dot(ns2)/norm2,nplan, -1./norm2,ns2);
Center.SetXYZ(pts1.XYZ()+ray1*ns1.XYZ());
if (!istgt) {
tgc.SetLinearForm(ray1,dnorm1w,tg1); // = tg1.Added(ray1*dn1w);
}
// On oriente ns1 du centre vers pts1, et ns2 du centre vers pts2
// et on rend le triedre ns1,ns2,nplan direct
if (ray1 > 0.) {
ns1.Reverse();
if (!istgt) {
dnorm1w.Reverse();
}
}
if (ray2 >0.) {
ns2.Reverse();
if (!istgt) {
dnorm2w.Reverse();
}
}
if (choix%2 != 0) {
np.Reverse();
dnp.Reverse();
}
if (!istgt) {
return GeomFill::GetCircle(myTConv,
ns1, ns2,
dnorm1w, dnorm2w,
np, dnp,
pts1, pts2,
tg1, tg2,
Abs(ray1), 0,
Center, tgc,
Poles,
DPoles,
Weights,
DWeights);
}
else {
GeomFill::GetCircle(myTConv,
ns1, ns2,
np, pts1, pts2,
Abs(ray1), Center,
Poles, Weights);
return Standard_False;
}
}
//=======================================================================
//function : Section
//purpose :
//=======================================================================
Standard_Boolean BlendFunc_ConstRad::Section
(const Blend_Point& P,
TColgp_Array1OfPnt& Poles,
TColgp_Array1OfVec& DPoles,
TColgp_Array1OfVec& D2Poles,
TColgp_Array1OfPnt2d& Poles2d,
TColgp_Array1OfVec2d& DPoles2d,
TColgp_Array1OfVec2d& D2Poles2d,
TColStd_Array1OfReal& Weights,
TColStd_Array1OfReal& DWeights,
TColStd_Array1OfReal& D2Weights)
{
gp_Vec ns1,ns2, np, dnp, d2np, dnorm1w, dnorm2w, d2norm1w, d2norm2w;
gp_Vec tgc, dtgc, dtg1, dtg2, temp, tempbis;
Standard_Real norm1, norm2;
gp_Pnt Center;
math_Vector X(1,4), sol(1,4), secmember(1,4);
math_Matrix D2DXdSdt(1,4,1,4);
Standard_Real prm = P.Parameter();
Standard_Integer low = Poles.Lower();
Standard_Integer upp = Poles.Upper();
Standard_Boolean istgt = Standard_True;
P.ParametersOnS1(X(1), X(2));
P.ParametersOnS2(X(3), X(4));
/* Pour debuger par des D.F
#if DEB
Standard_Real deltat = 1.e-7;
if (prm==tcurv->LastParameter()){deltat *= -1;} //Pour les discont
Standard_Real deltaX = 1.e-7;
Standard_Real seuil = 1.e-3;
Standard_Integer ii, jj;
gp_Vec d_plan, d1, d2, pdiff;
math_Matrix M(1,4,1,4), MDiff(1,4,1,4);
math_Matrix Mu1(1,4,1,4), Mv1(1,4,1,4);
math_Matrix Mu2(1,4,1,4), Mv2(1,4,1,4);
math_Vector V(1,4), VDiff(1,4),dx(1,4);
dx = X;
dx(1)+=deltaX;
ComputeValues(dx, 1, Standard_True, prm );
Mu1 = DEDX;
dx = X;
dx(2)+=deltaX;
ComputeValues(dx, 1, Standard_True, prm);
Mv1 = DEDX;
dx = X;
dx(3)+=deltaX;
ComputeValues(dx, 1, Standard_True, prm );
Mu2 = DEDX;
dx = X;
dx(4)+=deltaX;
ComputeValues(dx, 1, Standard_True, prm );
Mv2 = DEDX;
ComputeValues(X, 1, Standard_True, prm+deltat);
M = DEDX;
V = DEDT;
d_plan = dnplan;
d1 = dn1w;
d2 = dn2w;
# endif
*/
// Calculs des equations
ComputeValues(X, 2, Standard_True, prm);
distmin = Min (distmin, pts1.Distance(pts2));
/*
#if DEB
MDiff = (M - DEDX)*(1/deltat);
VDiff = (V - DEDT)*(1/deltat);
pdiff = (d_plan - dnplan)*(1/deltat);
if ((pdiff-d2nplan).Magnitude() > seuil*(pdiff.Magnitude()+1))
{
cout << "d2nplan = (" << d2nplan.X() << ","<< d2nplan.Y() << ","<< d2nplan.Z() << ")"<<endl;
cout << "Diff fi = (" << pdiff.X() << ","<< pdiff.Y() << ","<< pdiff.Z() << ")"<<endl;
}
pdiff = (d1 - dn1w)*(1/deltat);
if ((pdiff-d2n1w).Magnitude() > seuil*(pdiff.Magnitude()+1))
{
cout << "d2n1w = (" << d2n1w.X() << ","<< d2n1w.Y() << ","<< d2n1w.Z() << ")"<<endl;
cout << "Diff fi = (" << pdiff.X() << ","<< pdiff.Y() << ","<< pdiff.Z() << ")"<<endl;
}
pdiff = (d2 - dn2w)*(1/deltat);
if ((pdiff-d2n2w).Magnitude() > seuil*(pdiff.Magnitude()+1))
{
cout << "d2n2w = (" << d2n2w.X() << ","<< d2n2w.Y() << ","<< d2n2w.Z() << ")"<<endl;
cout << "Diff fi = (" << pdiff.X() << ","<< pdiff.Y() << ","<< pdiff.Z() << ")"<<endl;
}
for ( ii=1; ii<=4; ii++) {
if (Abs(VDiff(ii)-D2EDT2(ii)) > seuil*(Abs(D2EDT2(ii))+1))
{
cout << "erreur sur D2EDT2 : "<< ii << endl;
cout << D2EDT2(ii) << " D.F = " << VDiff(ii) << endl;
}
for (jj=1; jj<=4; jj++) {
if (Abs(MDiff(ii,jj)-D2EDXDT(ii, jj)) >
1.e-3*(Abs(D2EDXDT(ii, jj))+1.e-2))
{
cout << "erreur sur D2EDXDT : "<< ii << " , " << jj << endl;
cout << D2EDXDT(ii,jj) << " D.F = " << MDiff(ii,jj) << endl;
}
}
}
// Test de D2EDX2 en u1
MDiff = (Mu1 - DEDX)/deltaX;
for (ii=1; ii<=4; ii++) {
for (jj=1; jj<=4; jj++) {
if (Abs(MDiff(ii,jj)-D2EDX2(ii, jj, 1)) >
seuil*(Abs(D2EDX2(ii, jj, 1))+1))
{
cout << "erreur sur D2EDX2 : "<< ii << " , " << jj << " , " << 1 << endl;
cout << D2EDX2(ii,jj, 1) << " D.F = " << MDiff(ii,jj) << endl;
}
}
}
// Test de D2EDX2 en v1
MDiff = (Mv1 - DEDX)/deltaX;
for (ii=1; ii<=4; ii++) {
for (jj=1; jj<=4; jj++) {
if (Abs(MDiff(ii,jj)-D2EDX2(ii, jj, 2)) >
seuil*(Abs(D2EDX2(ii, jj, 2))+1))
{
cout << "erreur sur D2EDX2 : "<< ii << " , " << jj << " , " << 2 << endl;
cout << D2EDX2(ii,jj, 2) << " D.F = " << MDiff(ii,jj) << endl;
}
}
}
// Test de D2EDX2 en u2
MDiff = (Mu2 - DEDX)/deltaX;
for (ii=1; ii<=4; ii++) {
for (jj=1; jj<=4; jj++) {
if (Abs(MDiff(ii,jj)-D2EDX2(ii, jj, 3)) >
seuil*(Abs(D2EDX2(ii, jj, 3))+1))
{
cout << "erreur sur D2EDX2 : "<< ii << " , " << jj << " , " << 3 << endl;
cout << D2EDX2(ii,jj, 3) << " D.F = " << MDiff(ii,jj) << endl;
}
}
}
// Test de D2EDX2 en v2
MDiff = (Mv2 - DEDX)/deltaX;
for (ii=1; ii<=4; ii++) {
for (jj=1; jj<=4; jj++) {
if (Abs(MDiff(ii,jj)-D2EDX2(ii, jj, 4)) >
seuil*(Abs(D2EDX2(ii, jj, 4))+1))
{
cout << "erreur sur D2EDX2 : "<< ii << " , " << jj << " , " << 4 << endl;
cout << D2EDX2(ii,jj, 4) << " D.F = " << MDiff(ii,jj) << endl;
}
}
}
#endif
*/
// on recopie localement ns1, ns2, np afin de ne pas ecraser les champs !
ns1 = nsurf1;
ns2 = nsurf2;
np = nplan;
dnp = dnplan;
d2np = d2nplan;
// Calcul des derives
if ( ! pts1.IsEqual(pts2, 1.e-4)) {
math_Gauss Resol(DEDX, 1.e-9); // !Tol a affiner !!!!!
// Calcul des derives Traitement Normal
if (Resol.IsDone()) {
Resol.Solve(-DEDT, sol);
D2EDX2.Multiply(sol, D2DXdSdt);
secmember = - (D2EDT2 + (2*D2EDXDT+D2DXdSdt)*sol);
Resol.Solve(secmember);
istgt = Standard_False;
}
}
if (istgt) {
math_SVD SingRS (DEDX);
math_Vector Vbis(1,4);
if (SingRS.IsDone()) {
SingRS.Solve(-DEDT, sol, 1.e-6);
D2EDX2.Multiply(sol, D2DXdSdt);
Vbis = - (D2EDT2 + (2*D2EDXDT+D2DXdSdt)*sol);
SingRS.Solve( Vbis, secmember, 1.e-6);
istgt = Standard_False;
}
}
if (!istgt) {
tg1.SetLinearForm(sol(1),d1u1, sol(2),d1v1);
tg2.SetLinearForm(sol(3),d1u2, sol(4),d1v2);
dnorm1w.SetLinearForm(sol(1),dndu1, sol(2),dndv1, dn1w);
dnorm2w.SetLinearForm(sol(3),dndu2, sol(4),dndv2, dn2w);
temp.SetLinearForm(sol(1)*sol(1), d2u1,
2*sol(1)*sol(2), d2uv1,
sol(2)*sol(2), d2v1);
dtg1.SetLinearForm(secmember(1),d1u1, secmember(2),d1v1, temp);
temp.SetLinearForm(sol(3)*sol(3), d2u2,
2*sol(3)*sol(4), d2uv2,
sol(4)*sol(4), d2v2);
dtg2.SetLinearForm(secmember(3),d1u2, secmember(4),d1v2, temp);
temp.SetLinearForm(sol(1)*sol(1), d2ndu1,
2*sol(1)*sol(2), d2nduv1,
sol(2)*sol(2), d2ndv1);
tempbis.SetLinearForm(2*sol(1), d2ndtu1,
2*sol(2), d2ndtv1,
d2n1w);
temp+= tempbis;
d2norm1w.SetLinearForm(secmember(1),dndu1, secmember(2),dndv1, temp);
temp.SetLinearForm(sol(3)*sol(3), d2ndu2,
2*sol(3)*sol(4), d2nduv2,
sol(4)*sol(4), d2ndv2);
tempbis.SetLinearForm(2*sol(3), d2ndtu2,
2*sol(4), d2ndtv2,
d2n2w);
temp+= tempbis;
d2norm2w.SetLinearForm(secmember(3),dndu2, secmember(4),dndv2, temp);
}
// Les poles 2d
Poles2d(Poles2d.Lower()).SetCoord(X(1),X(2));
Poles2d(Poles2d.Upper()).SetCoord(X(3),X(4));
if (!istgt) {
DPoles2d(Poles2d.Lower()) .SetCoord(sol(1),sol(2));
DPoles2d(Poles2d.Upper()) .SetCoord(sol(3),sol(4));
D2Poles2d(Poles2d.Lower()).SetCoord(secmember(1), secmember(2));
D2Poles2d(Poles2d.Upper()).SetCoord(secmember(3), secmember(4));
}
// on traite le cas linear...
if (mySShape == BlendFunc_Linear) {
Poles(low) = pts1;
Poles(upp) = pts2;
Weights(low) = 1.0;
Weights(upp) = 1.0;
if (!istgt) {
DPoles(low) = tg1;
DPoles(upp) = tg2;
DPoles(low) = dtg1;
DPoles(upp) = dtg2;
DWeights(low) = 0.0;
DWeights(upp) = 0.0;
D2Weights(low) = 0.0;
D2Weights(upp) = 0.0;
}
return (!istgt);
}
// Cas du cercle
norm1 = nplan.Crossed(ns1).Magnitude();
norm2 = nplan.Crossed(ns2).Magnitude();
if (norm1 < Eps) {
norm1 = 1; // Insufisant, mais il ne faut pas planter
#if DEB
cout << " ConstRad : Surface singuliere " << endl;
#endif
}
if (norm2 < Eps) {
norm2 = 1; // Insufisant, mais il ne faut pas planter
#if DEB
cout << " ConstRad : Surface singuliere " << endl;
#endif
}
ns1.SetLinearForm(nplan.Dot(ns1)/norm1,nplan, -1./norm1, ns1);
ns2.SetLinearForm(nplan.Dot(ns2)/norm2,nplan, -1./norm2, ns2);
Center.SetXYZ(pts1.XYZ()+ray1*ns1.XYZ());
if (!istgt) {
tgc.SetLinearForm(ray1,dnorm1w,tg1);
dtgc.SetLinearForm(ray1, d2norm1w, dtg1);
}
// On oriente ns1 du centre vers pts1, et ns2 du centre vers pts2
// et on rend le triedre ns1,ns2,nplan direct
if (ray1 > 0.) {
ns1.Reverse();
if (!istgt) {
dnorm1w.Reverse();
d2norm1w.Reverse();
}
}
if (ray2 >0.) {
ns2.Reverse();
if (!istgt) {
dnorm2w.Reverse();
d2norm2w.Reverse();
}
}
if (choix%2 != 0) {
np.Reverse();
dnp.Reverse();
d2np.Reverse();
}
if (!istgt) {
return GeomFill::GetCircle(myTConv,
ns1, ns2,
dnorm1w, dnorm2w,
d2norm1w, d2norm2w,
np, dnp, d2np,
pts1, pts2,
tg1, tg2,
dtg1, dtg2,
Abs(ray1), 0, 0,
Center, tgc, dtgc,
Poles,
DPoles,
D2Poles,
Weights,
DWeights,
D2Weights);
}
else {
GeomFill::GetCircle(myTConv,
ns1, ns2,
nplan, pts1, pts2,
Abs(ray1), Center,
Poles, Weights);
return Standard_False;
}
}
//=======================================================================
//function : AxeRot
//purpose :
//=======================================================================
gp_Ax1 BlendFunc_ConstRad::AxeRot (const Standard_Real Prm)
{
gp_Ax1 axrot;
gp_Vec dirax, d1gui, d2gui, np, dnp;
gp_Pnt oriax, ptgui;
curv->D2(Prm,ptgui,d1gui,d2gui);
Standard_Real normtg = d1gui.Magnitude();
np = d1gui.Normalized();
dnp.SetLinearForm(1./normtg, d2gui,
-1./normtg*(np.Dot(d2gui)), np);
dirax = np.Crossed(dnp);
if (dirax.Magnitude() >= gp::Resolution()) {
axrot.SetDirection(dirax);
}
else {
axrot.SetDirection(np); // Pour ne pas planter
}
if (dnp.Magnitude() >= gp::Resolution()) {
oriax.SetXYZ(ptgui.XYZ()+
(normtg/dnp.Magnitude())*dnp.Normalized().XYZ());
}
else {
oriax.SetXYZ(ptgui.XYZ());
}
axrot.SetLocation(oriax);
return axrot;
}
void BlendFunc_ConstRad::Resolution(const Standard_Integer IC2d,
const Standard_Real Tol,
Standard_Real& TolU,
Standard_Real& TolV) const
{
if(IC2d == 1){
TolU = surf1->UResolution(Tol);
TolV = surf1->VResolution(Tol);
}
else {
TolU = surf2->UResolution(Tol);
TolV = surf2->VResolution(Tol);
}
}
View Git Blame Copy Permalink