/*
* Find dominator relationships.
* Assumes graph has been leveled.
*/
static void
find_dom(struct block *root)
{
int i;
struct block *b;
bpf_u_int32 *x;
/*
* Initialize sets to contain all nodes.
*/
x = all_dom_sets;
i = n_blocks * nodewords;
while (--i >= 0)
*x++ = ~0;
/* Root starts off empty. */
for (i = nodewords; --i >= 0;)
root->dom[i] = 0;
/* root->level is the highest level no found. */
for (i = root->level; i >= 0; --i) {
for (b = levels[i]; b; b = b->link) {
SET_INSERT(b->dom, b->id);
if (JT(b) == 0)
continue;
SET_INTERSECT(JT(b)->dom, b->dom, nodewords);
SET_INTERSECT(JF(b)->dom, b->dom, nodewords);
}
}
}
static void
opt_not(struct block *b)
{
struct block *tmp = JT(b);
JT(b) = JF(b);
JF(b) = tmp;
}
static void
opt_j(struct edge *ep)
{
register int i, k;
register struct block *target;
if (JT(ep->succ) == 0)
return;
if (JT(ep->succ) == JF(ep->succ)) {
/*
* Common branch targets can be eliminated, provided
* there is no data dependency.
*/
if (!use_conflict(ep->pred, ep->succ->et.succ)) {
done = 0;
ep->succ = JT(ep->succ);
}
}
/*
* For each edge dominator that matches the successor of this
* edge, promote the edge successor to the its grandchild.
*
* XXX We violate the set abstraction here in favor a reasonably
* efficient loop.
*/
top:
for (i = 0; i < edgewords; ++i) {
register bpf_u_int32 x = ep->edom[i];
while (x != 0) {
k = ffs(x) - 1;
x &=~ (1 << k);
k += i * BITS_PER_WORD;
target = fold_edge(ep->succ, edges[k]);
/*
* Check that there is no data dependency between
* nodes that will be violated if we move the edge.
*/
if (target != 0 && !use_conflict(ep->pred, target)) {
done = 0;
ep->succ = target;
if (JT(target) != 0)
/*
* Start over unless we hit a leaf.
*/
goto top;
return;
}
}
}
}
/*
* Return the number of nodes reachable by 'p'.
* All nodes should be initially unmarked.
*/
static int
count_blocks(struct block *p)
{
if (p == 0 || isMarked(p))
return 0;
Mark(p);
return count_blocks(JT(p)) + count_blocks(JF(p)) + 1;
}
static void
intern_blocks(struct block *root)
{
struct block *p;
int i, j;
int done1; /* don't shadow global */
top:
done1 = 1;
for (i = 0; i < n_blocks; ++i)
blocks[i]->link = 0;
mark_code(root);
for (i = n_blocks - 1; --i >= 0; ) {
if (!isMarked(blocks[i]))
continue;
for (j = i + 1; j < n_blocks; ++j) {
if (!isMarked(blocks[j]))
continue;
if (eq_blk(blocks[i], blocks[j])) {
blocks[i]->link = blocks[j]->link ?
blocks[j]->link : blocks[j];
break;
}
}
}
for (i = 0; i < n_blocks; ++i) {
p = blocks[i];
if (JT(p) == 0)
continue;
if (JT(p)->link) {
done1 = 0;
JT(p) = JT(p)->link;
}
if (JF(p)->link) {
done1 = 0;
JF(p) = JF(p)->link;
}
}
if (!done1)
goto top;
}
/*
* Return the number of stmts in the flowgraph reachable by 'p'.
* The nodes should be unmarked before calling.
*
* Note that "stmts" means "instructions", and that this includes
*
* side-effect statements in 'p' (slength(p->stmts));
*
* statements in the true branch from 'p' (count_stmts(JT(p)));
*
* statements in the false branch from 'p' (count_stmts(JF(p)));
*
* the conditional jump itself (1);
*
* an extra long jump if the true branch requires it (p->longjt);
*
* an extra long jump if the false branch requires it (p->longjf).
*/
static u_int
count_stmts(struct block *p)
{
u_int n;
if (p == 0 || isMarked(p))
return 0;
Mark(p);
n = count_stmts(JT(p)) + count_stmts(JF(p));
return slength(p->stmts) + n + 1 + p->longjt + p->longjf;
}
static void
make_marks(struct block *p)
{
if (!isMarked(p)) {
Mark(p);
if (BPF_CLASS(p->s.code) != BPF_RET) {
make_marks(JT(p));
make_marks(JF(p));
}
}
}
static void
find_levels_r(struct block *b)
{
int level;
if (isMarked(b))
return;
Mark(b);
b->link = 0;
if (JT(b)) {
find_levels_r(JT(b));
find_levels_r(JF(b));
level = MAX(JT(b)->level, JF(b)->level) + 1;
} else
level = 0;
b->level = level;
b->link = levels[level];
levels[level] = b;
}
static struct block *
fold_edge(struct block *child, struct edge *ep)
{
int sense;
int aval0, aval1, oval0, oval1;
int code = ep->code;
if (code < 0) {
code = -code;
sense = 0;
} else
sense = 1;
if (child->s.code != code)
return 0;
aval0 = child->val[A_ATOM];
oval0 = child->oval;
aval1 = ep->pred->val[A_ATOM];
oval1 = ep->pred->oval;
if (aval0 != aval1)
return 0;
if (oval0 == oval1)
/*
* The operands of the branch instructions are
* identical, so the result is true if a true
* branch was taken to get here, otherwise false.
*/
return sense ? JT(child) : JF(child);
if (sense && code == (BPF_JMP|BPF_JEQ|BPF_K))
/*
* At this point, we only know the comparison if we
* came down the true branch, and it was an equality
* comparison with a constant.
*
* I.e., if we came down the true branch, and the branch
* was an equality comparison with a constant, we know the
* accumulator contains that constant. If we came down
* the false branch, or the comparison wasn't with a
* constant, we don't know what was in the accumulator.
*
* We rely on the fact that distinct constants have distinct
* value numbers.
*/
return JF(child);
return 0;
}
/*
* Find the backwards transitive closure of the flow graph. These sets
* are backwards in the sense that we find the set of nodes that reach
* a given node, not the set of nodes that can be reached by a node.
*
* Assumes graph has been leveled.
*/
static void
find_closure(struct block *root)
{
int i;
struct block *b;
/*
* Initialize sets to contain no nodes.
*/
memset((char *)all_closure_sets, 0,
n_blocks * nodewords * sizeof(*all_closure_sets));
/* root->level is the highest level no found. */
for (i = root->level; i >= 0; --i) {
for (b = levels[i]; b; b = b->link) {
SET_INSERT(b->closure, b->id);
if (JT(b) == 0)
continue;
SET_UNION(JT(b)->closure, b->closure, nodewords);
SET_UNION(JF(b)->closure, b->closure, nodewords);
}
}
}
static void
opt_root(struct block **b)
{
struct slist *tmp, *s;
s = (*b)->stmts;
(*b)->stmts = 0;
while (BPF_CLASS((*b)->s.code) == BPF_JMP && JT(*b) == JF(*b))
*b = JT(*b);
tmp = (*b)->stmts;
if (tmp != 0)
sappend(s, tmp);
(*b)->stmts = s;
/*
* If the root node is a return, then there is no
* point executing any statements (since the bpf machine
* has no side effects).
*/
if (BPF_CLASS((*b)->s.code) == BPF_RET)
(*b)->stmts = 0;
}
/*
* Do a depth first search on the flow graph, numbering the
* the basic blocks, and entering them into the 'blocks' array.`
*/
static void
number_blks_r(struct block *p)
{
int n;
if (p == 0 || isMarked(p))
return;
Mark(p);
n = n_blocks++;
p->id = n;
blocks[n] = p;
number_blks_r(JT(p));
number_blks_r(JF(p));
}
static void
find_inedges(struct block *root)
{
int i;
struct block *b;
for (i = 0; i < n_blocks; ++i)
blocks[i]->in_edges = 0;
/*
* Traverse the graph, adding each edge to the predecessor
* list of its successors. Skip the leaves (i.e. level 0).
*/
for (i = root->level; i > 0; --i) {
for (b = levels[i]; b != 0; b = b->link) {
link_inedge(&b->et, JT(b));
link_inedge(&b->ef, JF(b));
}
}
}
/*
* Assume graph is already leveled.
*/
static void
find_ud(struct block *root)
{
int i, maxlevel;
struct block *p;
/*
* root->level is the highest level no found;
* count down from there.
*/
maxlevel = root->level;
for (i = maxlevel; i >= 0; --i)
for (p = levels[i]; p; p = p->link) {
compute_local_ud(p);
p->out_use = 0;
}
for (i = 1; i <= maxlevel; ++i) {
for (p = levels[i]; p; p = p->link) {
p->out_use |= JT(p)->in_use | JF(p)->in_use;
p->in_use |= p->out_use &~ p->kill;
}
}
}
/*
* Returns true if successful. Returns false if a branch has
* an offset that is too large. If so, we have marked that
* branch so that on a subsequent iteration, it will be treated
* properly.
*/
static int
convert_code_r(struct block *p)
{
struct bpf_insn *dst;
struct slist *src;
u_int slen;
u_int off;
int extrajmps; /* number of extra jumps inserted */
struct slist **offset = NULL;
if (p == 0 || isMarked(p))
return (1);
Mark(p);
if (convert_code_r(JF(p)) == 0)
return (0);
if (convert_code_r(JT(p)) == 0)
return (0);
slen = slength(p->stmts);
dst = ftail -= (slen + 1 + p->longjt + p->longjf);
/* inflate length by any extra jumps */
p->offset = dst - fstart;
/* generate offset[] for convenience */
if (slen) {
offset = (struct slist **)calloc(slen, sizeof(struct slist *));
if (!offset) {
bpf_error("not enough core");
/*NOTREACHED*/
}
}
src = p->stmts;
for (off = 0; off < slen && src; off++) {
#if 0
printf("off=%d src=%x\n", off, src);
#endif
offset[off] = src;
src = src->next;
}
off = 0;
for (src = p->stmts; src; src = src->next) {
if (src->s.code == NOP)
continue;
dst->code = (u_short)src->s.code;
dst->k = src->s.k;
/* fill block-local relative jump */
if (BPF_CLASS(src->s.code) != BPF_JMP || src->s.code == (BPF_JMP|BPF_JA)) {
#if 0
if (src->s.jt || src->s.jf) {
bpf_error("illegal jmp destination");
/*NOTREACHED*/
}
#endif
goto filled;
}
if (off == slen - 2) /*???*/
goto filled;
{
u_int i;
int jt, jf;
static const char ljerr[] = "%s for block-local relative jump: off=%d";
#if 0
printf("code=%x off=%d %x %x\n", src->s.code,
off, src->s.jt, src->s.jf);
#endif
if (!src->s.jt || !src->s.jf) {
bpf_error(ljerr, "no jmp destination", off);
/*NOTREACHED*/
}
jt = jf = 0;
for (i = 0; i < slen; i++) {
if (offset[i] == src->s.jt) {
if (jt) {
bpf_error(ljerr, "multiple matches", off);
/*NOTREACHED*/
}
dst->jt = i - off - 1;
jt++;
}
if (offset[i] == src->s.jf) {
if (jf) {
bpf_error(ljerr, "multiple matches", off);
/*NOTREACHED*/
}
dst->jf = i - off - 1;
jf++;
}
}
if (!jt || !jf) {
bpf_error(ljerr, "no destination found", off);
/*NOTREACHED*/
}
}
filled:
++dst;
++off;
}
if (offset)
free(offset);
#ifdef BDEBUG
bids[dst - fstart] = p->id + 1;
#endif
dst->code = (u_short)p->s.code;
dst->k = p->s.k;
if (JT(p)) {
extrajmps = 0;
off = JT(p)->offset - (p->offset + slen) - 1;
if (off >= 256) {
/* offset too large for branch, must add a jump */
if (p->longjt == 0) {
/* mark this instruction and retry */
p->longjt++;
return(0);
}
/* branch if T to following jump */
dst->jt = extrajmps;
extrajmps++;
dst[extrajmps].code = BPF_JMP|BPF_JA;
dst[extrajmps].k = off - extrajmps;
}
else
dst->jt = off;
off = JF(p)->offset - (p->offset + slen) - 1;
if (off >= 256) {
/* offset too large for branch, must add a jump */
if (p->longjf == 0) {
/* mark this instruction and retry */
p->longjf++;
return(0);
}
/* branch if F to following jump */
/* if two jumps are inserted, F goes to second one */
dst->jf = extrajmps;
extrajmps++;
dst[extrajmps].code = BPF_JMP|BPF_JA;
dst[extrajmps].k = off - extrajmps;
}
else
dst->jf = off;
}
return (1);
}
static void
and_pullup(struct block *b)
{
int val, at_top;
struct block *pull;
struct block **diffp, **samep;
struct edge *ep;
ep = b->in_edges;
if (ep == 0)
return;
/*
* Make sure each predecessor loads the same value.
*/
val = ep->pred->val[A_ATOM];
for (ep = ep->next; ep != 0; ep = ep->next)
if (val != ep->pred->val[A_ATOM])
return;
if (JT(b->in_edges->pred) == b)
diffp = &JT(b->in_edges->pred);
else
diffp = &JF(b->in_edges->pred);
at_top = 1;
while (1) {
if (*diffp == 0)
return;
if (JF(*diffp) != JF(b))
return;
if (!SET_MEMBER((*diffp)->dom, b->id))
return;
if ((*diffp)->val[A_ATOM] != val)
break;
diffp = &JT(*diffp);
at_top = 0;
}
samep = &JT(*diffp);
while (1) {
if (*samep == 0)
return;
if (JF(*samep) != JF(b))
return;
if (!SET_MEMBER((*samep)->dom, b->id))
return;
if ((*samep)->val[A_ATOM] == val)
break;
/* XXX Need to check that there are no data dependencies
between diffp and samep. Currently, the code generator
will not produce such dependencies. */
samep = &JT(*samep);
}
#ifdef notdef
/* XXX This doesn't cover everything. */
for (i = 0; i < N_ATOMS; ++i)
if ((*samep)->val[i] != pred->val[i])
return;
#endif
/* Pull up the node. */
pull = *samep;
*samep = JT(pull);
JT(pull) = *diffp;
/*
* At the top of the chain, each predecessor needs to point at the
* pulled up node. Inside the chain, there is only one predecessor
* to worry about.
*/
if (at_top) {
for (ep = b->in_edges; ep != 0; ep = ep->next) {
if (JT(ep->pred) == b)
JT(ep->pred) = pull;
else
JF(ep->pred) = pull;
}
}
else
*diffp = pull;
done = 0;
}
static void
opt_peep(struct block *b)
{
struct slist *s;
struct slist *next, *last;
int val;
s = b->stmts;
if (s == 0)
return;
last = s;
for (/*empty*/; /*empty*/; s = next) {
/*
* Skip over nops.
*/
s = this_op(s);
if (s == 0)
break; /* nothing left in the block */
/*
* Find the next real instruction after that one
* (skipping nops).
*/
next = this_op(s->next);
if (next == 0)
break; /* no next instruction */
last = next;
/*
* st M[k] --> st M[k]
* ldx M[k] tax
*/
if (s->s.code == BPF_ST &&
next->s.code == (BPF_LDX|BPF_MEM) &&
s->s.k == next->s.k) {
done = 0;
next->s.code = BPF_MISC|BPF_TAX;
}
/*
* ld #k --> ldx #k
* tax txa
*/
if (s->s.code == (BPF_LD|BPF_IMM) &&
next->s.code == (BPF_MISC|BPF_TAX)) {
s->s.code = BPF_LDX|BPF_IMM;
next->s.code = BPF_MISC|BPF_TXA;
done = 0;
}
/*
* This is an ugly special case, but it happens
* when you say tcp[k] or udp[k] where k is a constant.
*/
if (s->s.code == (BPF_LD|BPF_IMM)) {
struct slist *add, *tax, *ild;
/*
* Check that X isn't used on exit from this
* block (which the optimizer might cause).
* We know the code generator won't generate
* any local dependencies.
*/
if (ATOMELEM(b->out_use, X_ATOM))
continue;
/*
* Check that the instruction following the ldi
* is an addx, or it's an ldxms with an addx
* following it (with 0 or more nops between the
* ldxms and addx).
*/
if (next->s.code != (BPF_LDX|BPF_MSH|BPF_B))
add = next;
else
add = this_op(next->next);
if (add == 0 || add->s.code != (BPF_ALU|BPF_ADD|BPF_X))
continue;
/*
* Check that a tax follows that (with 0 or more
* nops between them).
*/
tax = this_op(add->next);
if (tax == 0 || tax->s.code != (BPF_MISC|BPF_TAX))
continue;
/*
* Check that an ild follows that (with 0 or more
* nops between them).
*/
ild = this_op(tax->next);
if (ild == 0 || BPF_CLASS(ild->s.code) != BPF_LD ||
BPF_MODE(ild->s.code) != BPF_IND)
continue;
/*
* We want to turn this sequence:
*
* (004) ldi #0x2 {s}
* (005) ldxms [14] {next} -- optional
* (006) addx {add}
* (007) tax {tax}
* (008) ild [x+0] {ild}
*
* into this sequence:
*
* (004) nop
* (005) ldxms [14]
* (006) nop
* (007) nop
* (008) ild [x+2]
*
* XXX We need to check that X is not
* subsequently used, because we want to change
* what'll be in it after this sequence.
*
* We know we can eliminate the accumulator
* modifications earlier in the sequence since
* it is defined by the last stmt of this sequence
* (i.e., the last statement of the sequence loads
* a value into the accumulator, so we can eliminate
* earlier operations on the accumulator).
*/
ild->s.k += s->s.k;
s->s.code = NOP;
add->s.code = NOP;
tax->s.code = NOP;
done = 0;
}
}
/*
* If the comparison at the end of a block is an equality
* comparison against a constant, and nobody uses the value
* we leave in the A register at the end of a block, and
* the operation preceding the comparison is an arithmetic
* operation, we can sometime optimize it away.
*/
if (b->s.code == (BPF_JMP|BPF_JEQ|BPF_K) &&
!ATOMELEM(b->out_use, A_ATOM)) {
/*
* We can optimize away certain subtractions of the
* X register.
*/
if (last->s.code == (BPF_ALU|BPF_SUB|BPF_X)) {
val = b->val[X_ATOM];
if (vmap[val].is_const) {
/*
* If we have a subtract to do a comparison,
* and the X register is a known constant,
* we can merge this value into the
* comparison:
*
* sub x -> nop
* jeq #y jeq #(x+y)
*/
b->s.k += vmap[val].const_val;
last->s.code = NOP;
done = 0;
} else if (b->s.k == 0) {
/*
* If the X register isn't a constant,
* and the comparison in the test is
* against 0, we can compare with the
* X register, instead:
*
* sub x -> nop
* jeq #0 jeq x
*/
last->s.code = NOP;
b->s.code = BPF_JMP|BPF_JEQ|BPF_X;
done = 0;
}
}
/*
* Likewise, a constant subtract can be simplified:
*
* sub #x -> nop
* jeq #y -> jeq #(x+y)
*/
else if (last->s.code == (BPF_ALU|BPF_SUB|BPF_K)) {
last->s.code = NOP;
b->s.k += last->s.k;
done = 0;
}
/*
* And, similarly, a constant AND can be simplified
* if we're testing against 0, i.e.:
*
* and #k nop
* jeq #0 -> jset #k
*/
else if (last->s.code == (BPF_ALU|BPF_AND|BPF_K) &&
b->s.k == 0) {
b->s.k = last->s.k;
b->s.code = BPF_JMP|BPF_K|BPF_JSET;
last->s.code = NOP;
done = 0;
opt_not(b);
}
}
/*
* jset #0 -> never
* jset #ffffffff -> always
*/
if (b->s.code == (BPF_JMP|BPF_K|BPF_JSET)) {
if (b->s.k == 0)
JT(b) = JF(b);
if (b->s.k == (int)0xffffffff)
JF(b) = JT(b);
}
/*
* If we're comparing against the index register, and the index
* register is a known constant, we can just compare against that
* constant.
*/
val = b->val[X_ATOM];
if (vmap[val].is_const && BPF_SRC(b->s.code) == BPF_X) {
bpf_int32 v = vmap[val].const_val;
b->s.code &= ~BPF_X;
b->s.k = v;
}
/*
* If the accumulator is a known constant, we can compute the
* comparison result.
*/
val = b->val[A_ATOM];
if (vmap[val].is_const && BPF_SRC(b->s.code) == BPF_K) {
bpf_int32 v = vmap[val].const_val;
switch (BPF_OP(b->s.code)) {
case BPF_JEQ:
v = v == b->s.k;
break;
case BPF_JGT:
v = (unsigned)v > (unsigned)b->s.k;
break;
case BPF_JGE:
v = (unsigned)v >= (unsigned)b->s.k;
break;
case BPF_JSET:
v &= b->s.k;
break;
default:
abort();
}
if (JF(b) != JT(b))
done = 0;
if (v)
JF(b) = JT(b);
else
JT(b) = JF(b);
}
}
int CGeometricConstraintSolver::performOnePass(CIKChainCont& chainCont,bool& limitOrAvoidanceNeedMoreCalculation,float interpolFact,float& nextInterpol,SGeomConstrSolverParam& parameters)
{ // Return value is bit-coded:
// bit0 set: at least one joint limitation was not respected
// bit1 set: max. angular or linear variations not respected.
// bit2 set: more than one joint limitation was not respected
// If return value is different from 0, the joints temp values are not actualized
// Here we have the multi-ik solving algorithm:
//********************************************************************************
limitOrAvoidanceNeedMoreCalculation=false;
// We prepare a vector of all used joints and a counter for the number of rows:
std::vector<CIKJoint*> allJoints;
int numberOfRows=0;
for (int elementNumber=0;elementNumber<int(chainCont.allChains.size());elementNumber++)
{
CIKChain* element=chainCont.allChains[elementNumber];
numberOfRows=numberOfRows+element->matrix->rows;
for (int i=0;i<int(element->rowJoints.size());i++)
{
CIKJoint* current=element->rowJoints[i];
// We check if that joint is already present:
bool present=false;
for (int j=0;j<int(allJoints.size());j++)
{
if (allJoints[j]==current)
{
present=true;
break;
}
}
if (!present)
allJoints.push_back(current);
}
}
// Now we prepare the individual joint constraints part: (part1)
//---------------------------------------------------------------------------
for (int i=0;i<int(allJoints.size());i++)
{
if (allJoints[i]->graphJoint->followedJoint!=NULL)
numberOfRows++;
}
//---------------------------------------------------------------------------
// We prepare the main matrix and the main error vector.
CMatrix mainMatrix(numberOfRows,allJoints.size());
// We have to zero it first:
mainMatrix.clear();
CMatrix mainErrorVector(numberOfRows,1);
// Now we fill in the main matrix and the main error vector:
int currentRow=0;
for (int elementNumber=0;elementNumber<int(chainCont.allChains.size());elementNumber++)
{
CIKChain* element=chainCont.allChains[elementNumber];
for (int i=0;i<element->errorVector->rows;i++)
{ // We go through the rows:
// We first set the error part:
mainErrorVector(currentRow,0)=(*element->errorVector)(i,0);
// Now we set the delta-parts:
for (int j=0;j<element->matrix->cols;j++)
{ // We go through the columns:
// We search for the right entry
CIKJoint* thisJoint=element->rowJoints[j];
int index=0;
while (allJoints[index]!=thisJoint)
index++;
mainMatrix(currentRow,index)=(*element->matrix)(i,j);
}
currentRow++;
}
}
// Now we prepare the individual joint constraints part: (part2)
//---------------------------------------------------------------------------
for (int i=0;i<int(allJoints.size());i++)
{
CIKGraphJoint* originalGraphJoint=allJoints[i]->graphJoint;
CIKGraphJoint* dependenceGraphJoint=originalGraphJoint->followedJoint;
if (dependenceGraphJoint!=NULL)
{
bool found=false;
int j;
for (j=0;j<int(allJoints.size());j++)
{
if (allJoints[j]->graphJoint==dependenceGraphJoint)
{
found=true;
break;
}
}
if (found)
{
float coeff=originalGraphJoint->coefficientValue;
float fact=originalGraphJoint->constantValue;
mainErrorVector(currentRow,0)=(allJoints[i]->tempParameter-fact-coeff*allJoints[j]->tempParameter)*interpolFact;
mainMatrix(currentRow,i)=-1.0f;
mainMatrix(currentRow,j)=coeff;
}
else
{ // joint of dependenceID is not part of this group calculation:
// therefore we take its current value
float coeff=originalGraphJoint->coefficientValue;
float fact=originalGraphJoint->constantValue;
mainErrorVector(currentRow,0)=(allJoints[i]->tempParameter-fact-coeff*dependenceGraphJoint->parameter)*interpolFact;
mainMatrix(currentRow,i)=-1.0f;
}
currentRow++;
}
}
//---------------------------------------------------------------------------
// We take the joint weights into account here (part1):
for (int i=0;i<mainMatrix.rows;i++)
{
for (int j=0;j<int(allJoints.size());j++)
{
float coeff=allJoints[j]->weight;
if (coeff>=0.0f)
coeff=sqrtf(coeff);
else
coeff=-sqrtf(-coeff);
mainMatrix(i,j)=mainMatrix(i,j)*coeff;
}
}
// Now we just have to solve:
int doF=mainMatrix.cols;
int eqNumb=mainMatrix.rows;
CMatrix solution(doF,1);
//************************************** RESOLUTION ***************************************
CMatrix JT(mainMatrix.rows,mainMatrix.cols);
JT=mainMatrix;
JT.transpose();
CMatrix DLSJ(doF,eqNumb);
CMatrix JJTInv(eqNumb,eqNumb);
JJTInv=mainMatrix*JT;
CMatrix ID(mainMatrix.rows,mainMatrix.rows);
ID.setIdentity();
for (int i=0;i<ID.rows;i++)
ID(i,i)=0.0f;
int rowPos=0;
for (int elementNumber=0;elementNumber<int(chainCont.allChains.size());elementNumber++)
{
CIKChain* element=chainCont.allChains[elementNumber];
for (int i=0;i<element->errorVector->rows;i++)
{
float damping=element->tooltip->dampingFactor+parameters.generalDamping;
ID(rowPos,rowPos)=damping*damping;
rowPos++;
}
}
JJTInv+=ID;
if (!JJTInv.inverse())
return(false); // error occured (matrix not invertible, nan numbers or such!)
DLSJ=JT*JJTInv;
solution=DLSJ*mainErrorVector;
//*****************************************************************************************
// We take the joint weights into account here (part2) and prepare the probable delta-values:
for (int i=0;i<doF;i++)
{
float coeff=sqrtf(fabs(allJoints[i]->weight));
solution(i,0)=solution(i,0)*coeff;
allJoints[i]->probableDeltaValue=solution(i,0);
}
// We check if some variations are too big:
int returnValue=0;
int lockJointNb=-1;
for (int i=0;i<doF;i++)
{
CIKJoint* it=allJoints[i];
if (it->revolute)
{
if (fabs(it->probableDeltaValue)>parameters.maxAngularVariation)
returnValue=returnValue|2;
}
else
{
if (fabs(it->probableDeltaValue)>parameters.maxLinearVariation)
returnValue=returnValue|2;
}
// ******************** This is for joint limitation *********************
bool doIt=true;
if (it->spherical)
{
if (it->topJoint!=NULL)
it=it->topJoint;
else
doIt=false;
}
if (doIt)
{
float overV=it->getValueOverLimitation(false);
if (overV>-0.0001f)
{
if (overV>0.99f)
overV=fabs(1000000.0f*it->probableDeltaValue)+1.0f;
if ((returnValue&1)!=0)
{
if (overV>nextInterpol)
{
nextInterpol=overV;
lockJointNb=i;
}
returnValue|=4;
}
else
{
nextInterpol=overV;
returnValue|=1;
lockJointNb=i;
}
}
}
// *************************************************************************
}
if (lockJointNb!=-1)
{
CIKJoint* it=allJoints[lockJointNb];
if (it->spherical)
{
if (it->topJoint!=NULL)
it=it->topJoint;
}
it->getValueOverLimitation(true);
// Now we have to lock all joints which are linked to that one through a
// linear equation:
std::vector<CIKJoint*> lockDependent;
lockDependent.push_back(it);
while (lockDependent.size()!=0)
{
it=lockDependent.back();
lockDependent.pop_back();
for (int i=0;i<int(allJoints.size());i++)
{
if ( (allJoints[i]->active)&&(allJoints[i]->graphJoint->followedJoint==it->graphJoint) )
{
CIKJoint* it2=allJoints[i];
it2->tempParameter=it2->graphJoint->constantValue+it2->graphJoint->coefficientValue*it->tempParameter;
if (!it2->cyclic)
{
if (it2->tempParameter<it2->minValue)
it2->tempParameter=it2->minValue;
if (it2->tempParameter>(it2->minValue+it2->range))
it2->tempParameter=it2->minValue+it2->range;
}
it2->active=false;
it2->copyStateToAvatarKids();
lockDependent.push_back(it2);
}
}
}
}
if (returnValue==0)
{ // Now we set the computed values
for (int i=0;i<doF;i++)
{
CIKJoint* it=allJoints[i];
if (it->active)
{
it->tempParameter+=it->probableDeltaValue;
it->copyStateToAvatarKids();
}
}
}
return(returnValue);
}
