// recursive subtracks fixer for appending a single point
void Track::InsertSubTracks(LLBBox &box, int level, int pos)
{
if(level == (int)SubTracks.size()) {
std::vector <SubTrack> new_level;
if(level > 0)
box.Expand(SubTracks[level-1][0].m_box);
new_level.push_back(SubTrack());
new_level[pos].m_box = box;
SubTracks.push_back(new_level);
} else
if(pos < (int)SubTracks[level].size())
SubTracks[level][pos].m_box.Expand(box);
else {
SubTracks[level].push_back(SubTrack());
SubTracks[level][pos].m_box = box;
}
if(level == 0)
SubTracks[level][pos].m_scale = 0;
else {
int left = pos << level;
int right = wxMin(left + (1 << level), TrackPoints.size() - 1);
SubTracks[level][pos].m_scale = ComputeScale(left, right);
}
if(pos > 0)
InsertSubTracks(box, level + 1, pos >> 1);
}
void ComputeAbsolute(face_t* f, vec3_t& p1, vec3_t& p2, vec3_t& p3)
{
vec3_t ex,ey,ez; // local axis base
#ifdef _DEBUG
if (g_qeglobals.m_bBrushPrimitMode)
Sys_Printf("Warning : illegal call of ComputeAbsolute in brush primitive mode\n");
#endif
// compute first local axis base
TextureAxisFromPlane(&f->plane, ex, ey);
CrossProduct(ex, ey, ez);
vec3_t aux;
VectorCopy(ex, aux);
VectorScale(aux, -f->texdef.shift[0], aux);
VectorCopy(aux, p1);
VectorCopy(ey, aux);
VectorScale(aux, -f->texdef.shift[1], aux);
VectorAdd(p1, aux, p1);
VectorCopy(p1, p2);
VectorAdd(p2, ex, p2);
VectorCopy(p1, p3);
VectorAdd(p3, ey, p3);
VectorCopy(ez, aux);
VectorScale(aux, -f->texdef.rotate, aux);
VectorRotate(p1, aux, p1);
VectorRotate(p2, aux, p2);
VectorRotate(p3, aux, p3);
// computing rotated local axis base
vec3_t rex,rey;
VectorCopy(ex, rex);
VectorRotate(rex, aux, rex);
VectorCopy(ey, rey);
VectorRotate(rey, aux, rey);
ComputeScale(rex,rey,p1,f);
ComputeScale(rex,rey,p2,f);
ComputeScale(rex,rey,p3,f);
// project on normal plane
// along ez
// assumes plane normal is normalized
ProjectOnPlane(f->plane.normal,f->plane.dist,ez,p1);
ProjectOnPlane(f->plane.normal,f->plane.dist,ez,p2);
ProjectOnPlane(f->plane.normal,f->plane.dist,ez,p3);
};
void ComputeAbsolute( face_s* f, edVec3_c& p1, edVec3_c& p2, edVec3_c& p3 )
{
edVec3_c ex, ey, ez; // local axis base
#ifdef _DEBUG
if ( g_qeglobals.m_bBrushPrimitMode )
Sys_Printf( "Warning : illegal call of ComputeAbsolute in brush primitive mode\n" );
#endif
// compute first local axis base
TextureAxisFromPlane( f->plane, ex, ey );
ez.crossProduct( ex, ey );
edVec3_c aux = ex * -f->texdef.shift[0];
p1 = aux;
aux = ey * -f->texdef.shift[1];
p1 += aux;
p2 = p1;
p2 += ex;
p3 = p1;
p3 += ey;
aux = ez;
aux *= -f->texdef.rotate;
VectorRotate( p1, aux, p1 );
VectorRotate( p2, aux, p2 );
VectorRotate( p3, aux, p3 );
// computing rotated local axis base
edVec3_c rex = ex;
VectorRotate( rex, aux, rex );
edVec3_c rey = ey;
VectorRotate( rey, aux, rey );
ComputeScale( rex, rey, p1, f );
ComputeScale( rex, rey, p2, f );
ComputeScale( rex, rey, p3, f );
// project on normal plane
// along ez
// assumes plane normal is normalized
f->plane.projectOnPlane( ez, p1 );
f->plane.projectOnPlane( ez, p2 );
f->plane.projectOnPlane( ez, p3 );
};
void ComputeAbsolute(face_t* f, vec3_t& p1, vec3_t& p2, vec3_t& p3)
{
vec3_t ex,ey,ez; // local axis base
// compute first local axis base
TextureAxisFromPlane(&f->plane, ex, ey);
CrossProduct(ex, ey, ez);
vec3_t aux;
VectorCopy(ex, aux);
VectorScale(aux, -f->texdef.shift[0], aux);
VectorCopy(aux, p1);
VectorCopy(ey, aux);
VectorScale(aux, -f->texdef.shift[1], aux);
VectorAdd(p1, aux, p1);
VectorCopy(p1, p2);
VectorAdd(p2, ex, p2);
VectorCopy(p1, p3);
VectorAdd(p3, ey, p3);
VectorCopy(ez, aux);
VectorScale(aux, -f->texdef.rotate, aux);
VectorRotate(p1, aux, p1);
VectorRotate(p2, aux, p2);
VectorRotate(p3, aux, p3);
// computing rotated local axis base
vec3_t rex,rey;
VectorCopy(ex, rex);
VectorRotate(rex, aux, rex);
VectorCopy(ey, rey);
VectorRotate(rey, aux, rey);
ComputeScale(rex,rey,p1,f);
ComputeScale(rex,rey,p2,f);
ComputeScale(rex,rey,p3,f);
// project on normal plane
// along ez
// assumes plane normal is normalized
ProjectOnPlane(f->plane.normal,f->plane.dist,ez,p1);
ProjectOnPlane(f->plane.normal,f->plane.dist,ez,p2);
ProjectOnPlane(f->plane.normal,f->plane.dist,ez,p3);
};
/* ensures the SubTracks are valid for assembly use */
void Track::Finalize()
{
if(SubTracks.size()) // subtracks already computed
return;
// OCPNStopWatch sw1;
int n = TrackPoints.size() - 1;
int level = 0;
while(n > 0) {
std::vector <SubTrack> new_level;
new_level.resize(n);
if(level == 0)
for(int i=0; i<n; i++) {
new_level[i].m_box.SetFromSegment(TrackPoints[i]->m_lat,
TrackPoints[i]->m_lon,
TrackPoints[i+1]->m_lat,
TrackPoints[i+1]->m_lon);
new_level[i].m_scale = 0;
}
else {
for(int i=0; i<n; i++) {
int p = i<<1;
new_level[i].m_box = SubTracks[level-1][p].m_box;
if(p+1 < (int)SubTracks[level-1].size())
new_level[i].m_box.Expand(SubTracks[level-1][p+1].m_box);
int left = i << level;
int right = wxMin(left + (1 << level), TrackPoints.size() - 1);
new_level[i].m_scale = ComputeScale(left, right);
}
}
SubTracks.push_back(new_level);
if(n > 1 && n&1)
n++;
n >>= 1;
level++;
}
// if(TrackPoints.size() > 100)
// printf("fin time %f %d\n", sw1.GetTime(), (int)TrackPoints.size());
}
bool IK_QJacobianSolver::Solve(
IK_QSegment *root,
std::list<IK_QTask *> tasks,
const MT_Scalar,
const int max_iterations
)
{
float scale = ComputeScale();
bool solved = false;
//double dt = analyze_time();
Scale(scale, tasks);
ConstrainPoleVector(root, tasks);
root->UpdateTransform(m_rootmatrix);
// iterate
for (int iterations = 0; iterations < max_iterations; iterations++) {
// update transform
root->UpdateTransform(m_rootmatrix);
std::list<IK_QTask *>::iterator task;
// compute jacobian
for (task = tasks.begin(); task != tasks.end(); task++) {
if ((*task)->Primary())
(*task)->ComputeJacobian(m_jacobian);
else
(*task)->ComputeJacobian(m_jacobian_sub);
}
MT_Scalar norm = 0.0;
do {
// invert jacobian
try {
m_jacobian.Invert();
if (m_secondary_enabled)
m_jacobian.SubTask(m_jacobian_sub);
}
catch (...) {
fprintf(stderr, "IK Exception\n");
return false;
}
// update angles and check limits
} while (UpdateAngles(norm));
// unlock segments again after locking in clamping loop
std::vector<IK_QSegment *>::iterator seg;
for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
(*seg)->UnLock();
// compute angle update norm
MT_Scalar maxnorm = m_jacobian.AngleUpdateNorm();
if (maxnorm > norm)
norm = maxnorm;
// check for convergence
if (norm < 1e-3 && iterations > 10) {
solved = true;
break;
}
}
if (m_poleconstraint)
root->PrependBasis(m_rootmatrix.getBasis());
Scale(1.0f / scale, tasks);
//analyze_add_run(max_iterations, analyze_time()-dt);
return solved;
}
