void LDLDecomposition<T>::set(const MatrixT& A)
{
Assert(A.m == A.n);
LDL.resize(A.n,A.n);
int i,j,k;
T sum;
for(i=0;i<A.n;i++) {
sum = A(i,i);
for(k=0;k<i;k++) sum -= LDL(k,k)*Sqr(LDL(i,k));
LDL(i,i) = sum;
if(FuzzyZero(sum,zeroTolerance)) {
/*
if(verbose >= 1)
LOG4CXX_ERROR(KrisLibrary::logger(),"Warning: LDLt decomposition has a zero element on diagonal "<<i);
*/
}
for(j=i+1;j<A.n;j++) {
sum = A(i,j);
for(int k=0;k<i;k++) sum -= LDL(k,k)*LDL(i,k)*LDL(j,k);
if(LDL(i,i) == 0) {
if(!FuzzyZero(sum,zeroTolerance)) {
if(verbose >= 1) LOG4CXX_ERROR(KrisLibrary::logger(),"LDLDecomposition: Zero diagonal, what to do with sum "<<sum<<"?");
sum = 0;
}
}
else
sum /= LDL(i,i);
LDL(j,i) = LDL(i,j) = sum;
}
}
/*
MatrixT L,LD,LDLt;
VectorT D;
getL(L);
getD(D);
//LOG4CXX_INFO(KrisLibrary::logger(),"A: "); A.print();
//LOG4CXX_INFO(KrisLibrary::logger(),"L: "); L.print();
//LOG4CXX_INFO(KrisLibrary::logger(),"D: "); D.print();
LD = L;
for(int i=0;i<A.n;i++)
LD.scaleCol(i,LDL(i,i));
LDLt.mulTransposeB(LD,L);
//LOG4CXX_INFO(KrisLibrary::logger(),"LDLt: "); LDLt.print();
LDLt -= A;
LOG4CXX_ERROR(KrisLibrary::logger(),"Max error in LDL "<<LDLt.maxAbsElement());
*/
}
void SparseVectorCompressed<T>::set(const T* v,int n,T zeroTol)
{
int nnz=0;
for(int i=0;i<this->n;i++)
if(!FuzzyZero(v[i],zeroTol)) nnz++;
resize(n,nnz);
nnz=0;
for(int i=0;i<this->n;i++) {
if(!FuzzyZero(v[i],zeroTol)) {
vals[nnz] = v[i];
indices[nnz]=i;
nnz++;
}
}
}
void LDLDecomposition<T>::getPseudoInverse(MatrixT& Ainv) const
{
Ainv.resize(LDL.n,LDL.n);
VectorT temp(LDL.n,Zero),y,x;
for(int i=0;i<LDL.n;i++) {
temp(i)=One;
LBackSub(temp,y);
for(int j=0;j<y.n;j++) {
if(!FuzzyZero(LDL(j,j),zeroTolerance))
y(j) = y(j)/LDL(j,j);
else
y(j) = 0.0;
}
LTBackSub(y,x);
//fill in a column
for(int j=0;j<LDL.n;j++)
Ainv(j,i)=x(j);
temp(i)=Zero;
}
T tol = Ainv.maxAbsElement()*Epsilon;
for(int i=0;i<LDL.n;i++)
for(int j=0;j<i;j++) {
if(!FuzzyEquals(Ainv(i,j),Ainv(j,i),tol))
LOG4CXX_INFO(KrisLibrary::logger(),Ainv);
Assert(FuzzyEquals(Ainv(i,j),Ainv(j,i),tol));
Ainv(i,j)=Ainv(j,i) = 0.5*(Ainv(i,j)+Ainv(j,i));
}
}
bool LeastSquares1D::SolvePerpendicular()
{
//minimize sum_i { (yi - a*xi - b)^2 / (1+b^2) }
assert(x.n == y.n);
assert(x.n >= 2);
Real sxx,syy,sxy;
Real xmean = Mean(x);
Real ymean = Mean(y);
sxx = syy = sxy = 0;
for(int i=0;i<x.n;i++) {
sxx += Sqr(x(i) - xmean);
syy += Sqr(y(i) - ymean);
sxy += (x(i) - xmean)*(y(i) - ymean);
}
Real den = (x.n*xmean*ymean - dot(x,y));
if(FuzzyZero(den)) {
cerr<<"Warning, denominator is close to zero"<<endl;
}
Real B = (syy - sxx) / den;
a = -B + Sqrt(Sqr(B)+1);
b = ymean - xmean*b;
cerr<<"Warning, don't know how to calculate std errors for perpendicular least-squares"<<endl;
stda = stdb = 0;
corrCoeff = 1;
return true;
}
bool LBackSubstitute(const MatrixTemplate<T>& a, const VectorTemplate<T>& b, VectorTemplate<T>& x)
{
Assert(a.isSquare());
Assert(a.n == b.n);
if(x.empty()) x.resize(a.n);
else Assert(a.n == x.n);
int n=a.n;
T aii,sum;
for(int i=0; i<n; i++) {
aii=a(i,i);
sum=b[i];
for(int j=0; j<i; j++)
sum-=a(i,j)*x[j];
if(aii == 0) {
if(!FuzzyZero(sum,(T)kBackSubZeroTolerance)) {
//cerr<<"LBackSubstitute: dividing by zero: "<<sum<<"/"<<aii<<endl;
//cerr<<MatrixPrinter(a)<<endl;
return false;
}
x[i]=0;
}
else
x[i]=sum/aii;
}
return true;
}
bool LtBackSubstitute(const MatrixTemplate<T>& a, const VectorTemplate<T>& b, VectorTemplate<T>& x)
{
Assert(a.isSquare());
Assert(a.n == b.n);
if(x.empty()) x.resize(a.n);
else Assert(a.n == x.n);
int n=a.n;
T aii,sum;
for(int i=n-1; i>=0; i--) {
aii=a(i,i);
sum=b[i];
for(int j=i+1; j<n; j++)
sum-=a(j,i)*x[j];
if(aii == 0) {
if(!FuzzyZero(sum,(T)kBackSubZeroTolerance)) {
//LOG4CXX_ERROR(KrisLibrary::logger(),"LtBackSubstitute: dividing by zero: "<<sum<<"/"<<aii);
return false;
}
x[i]=0;
}
else
x[i]=sum/aii;
}
return true;
}
void SparseVectorTemplate<T>::set(const T* v,int n,T zeroTol)
{
this->resize(n);
this->entries.clear();
for(int i=0;i<n;i++) {
if(!FuzzyZero(v[i],zeroTol)) BaseT::push_back(i,v[i]);
}
}
void SparseVectorTemplate<T>::set(const VectorT& v,T zeroTol)
{
BaseT::resize(v.n);
this->entries.clear();
for(int i=0;i<v.n;i++) {
if(!FuzzyZero(v(i),zeroTol)) BaseT::push_back(i,v(i));
}
}
static float ClampAngle(const float minimum, const float maximum, const float result, const float source)
{
if(FuzzyZero(minimum) && FuzzyZero(maximum))
{
return 0;
}
else if(result < minimum)
{
return minimum;
}
else if(result > maximum)
{
return maximum;
}
return source;
}
void AxisRotationFit(const std::vector<Vector3>& a,const std::vector<Vector3>& b,const Vector3& z,Real& theta)
{
Assert(!a.empty());
Assert(a.size()==b.size());
//min sum||R*a[i]-b[i]||^2
// = sum (R*a[i]-b[i]).(R*a[i]-b[i])
// = sum a[i].a[i] + b[i].b[i] - 2 b[i].R*a[i]
//differentiating, get
// 0 = sum b[i].R'*a[i] = sum b[i].[z]R*a[i]
//Let s=sin(theta) and c=cos(theta).
// R = cI + (1-c)zz' + s[z]
//so
// 0 = sum c*b[i]'[z]a[i] + (1-c)b[i]'[z]zz'a[i] + sb[i]'[z][z]a[i]
// = c*sum b[i]'[z]a[i] + s*b[i]'[z][z]a[i]
// [z] = [0 -z y]
// [z 0 -x]
// [-y x 0]
// [z]^2 = [-zz-yy xy xz ]
// [xy -xx-zz yz ]
// [xz yz -xx-yy]
//collecting terms by s and c,
//get s(sum -axbx-ayby) + c(sum axby-aybx) = 0
//solve using theta = atan(sum axby-aybx / sum -axbx-ayby)
//is it theta or theta+pi?
Matrix3 zcross,zcross2;
zcross.setCrossProduct(z);
zcross2.mul(zcross,zcross);
Real scoeff=0,ccoeff=0;
for(size_t i=0;i<a.size();i++)
scoeff += b[i].dot(zcross2*a[i]);
for(size_t i=0;i<b.size();i++)
ccoeff += b[i].dot(zcross*a[i]);
if(FuzzyZero(scoeff) && FuzzyZero(ccoeff))
theta=0;
else
theta = Atan2(-ccoeff,scoeff);
Real c=Cos(theta),s=Sin(theta);
if(c*scoeff-s*ccoeff > 0) {
theta += Pi;
}
}
bool DiagonalMatrixTemplate<T>::isInvertible(T eps) const
{
if(this->empty())
FatalError(MatrixError_SizeZero);
ItT v=this->begin();
for(int i=0; i<this->n; i++,v++)
if(FuzzyZero(*v,eps))
return false;
return true;
}
void SparseVectorTemplate<T>::copySubVector(int i,const VectorT& v,T zeroTol)
{
Assert(i >= 0);
Assert(i+v.n <= (int)this->n);
for(int j=0;j<v.n;j++) {
if(FuzzyZero(v(j),zeroTol))
this->erase(i+j);
else
this->insert(i+j,v(j));
}
}
bool LDLDecomposition<T>::DBackSub(const VectorT& b, VectorT& x) const
{
bool res=true;
x.resize(b.n);
Assert(b.n==x.n);
for(int i=0;i<x.n;i++) {
if(!FuzzyZero(LDL(i,i),zeroTolerance))
x(i) = b(i)/LDL(i,i);
else {
if(!FuzzyZero(b(i),zeroTolerance)) {
if(verbose >= 1)
LOG4CXX_ERROR(KrisLibrary::logger(),"LDLDecomposition::DBackSub(): Warning, zero on the diagonal, b("<<i<<")="<<b(i));
res = false;
x(i) = Sign(b(i))*Inf;
}
else
x(i) = 0;
}
}
return res;
}
void LDLDecomposition<T>::set(const MatrixT& A)
{
Assert(A.m == A.n);
LDL.resize(A.n,A.n);
int i,j,k;
T sum;
for(i=0;i<A.n;i++) {
sum = A(i,i);
for(k=0;k<i;k++) sum -= LDL(k,k)*Sqr(LDL(i,k));
LDL(i,i) = sum;
if(FuzzyZero(sum,zeroTolerance)) {
cerr<<"Warning: LDLt decomposition has a zero element on diagonal "<<i<<endl;
}
for(j=i+1;j<A.n;j++) {
sum = A(i,j);
for(int k=0;k<i;k++) sum -= LDL(k,k)*LDL(i,k)*LDL(j,k);
if(LDL(i,i) == 0) {
if(sum != 0) {
cerr<<"Zero diagonal, what to do with sum "<<sum<<endl;
sum = 0;
}
}
else
sum /= LDL(i,i);
LDL(j,i) = LDL(i,j) = sum;
}
}
/*
MatrixT L,LD,LDLt;
VectorT D;
getL(L);
getD(D);
//cout<<"A: "; A.print();
//cout<<"L: "; L.print();
//cout<<"D: "; D.print();
LD = L;
for(int i=0;i<A.n;i++)
LD.scaleCol(i,LDL(i,i));
LDLt.mulTransposeB(LD,L);
//cout<<"LDLt: "; LDLt.print();
LDLt -= A;
cout<<"Max error in LDL "<<LDLt.maxAbsElement()<<endl;
*/
}
void PmxMaterialMergeMorph(PMX_MATERIAL* material, MORPH_MATERIAL* morph, FLOAT_T weight)
{
CLAMPED(weight, 0, 1);
if(FuzzyZero((float)weight) != 0)
{
PmxMaterialResetMorph(material);
}
else
{
switch(morph->operation)
{
case 0: // 乗算
MaterialRGB3CalculateMultiWeight(&material->ambient, morph->ambient, weight);
MaterialRGBA3CalculateMultiWeight(&material->diffuse, morph->diffuse, weight);
MaterialRGB3CalculateMultiWeight(&material->specular, morph->specular, weight);
material->shininess[1] = (float)(1 - (1 - morph->shininess) * weight);
MaterialRGBA3CalculateMultiWeight(&material->edge_color, morph->edge_color, weight);
material->edge_size[1] = (float)(1 - (1 - morph->edge_size) * weight);
MaterialRGBA3CalculateMultiWeight(&material->main_texture_blend, morph->texture_weight, weight);
MaterialRGBA3CalculateMultiWeight(&material->sphere_texture_blend, morph->sphere_texture_weight, weight);
MaterialRGBA3CalculateMultiWeight(&material->toon_texture_blend, morph->toon_texture_weight, weight);
break;
case 1: // 加算
MaterialRGB3CalculateAddWeight(&material->ambient, morph->ambient, weight);
MaterialRGBA3CalculateAddWeight(&material->diffuse, morph->diffuse, weight);
MaterialRGB3CalculateAddWeight(&material->specular, morph->specular, weight);
material->shininess[2] = (float)(morph->shininess * weight);
MaterialRGBA3CalculateAddWeight(&material->edge_color, morph->edge_color, weight);
material->edge_size[2] = (float)(morph->edge_size * weight);
MaterialRGBA3CalculateAddWeight(&material->main_texture_blend, morph->texture_weight, weight);
MaterialRGBA3CalculateAddWeight(&material->sphere_texture_blend, morph->sphere_texture_weight, weight);
MaterialRGBA3CalculateAddWeight(&material->toon_texture_blend, morph->toon_texture_weight, weight);
break;
}
MaterialRGB3Calculate(&material->ambient);
MaterialRGBA3Calculate(&material->diffuse);
MaterialRGB3Calculate(&material->specular);
MaterialRGBA3Calculate(&material->edge_color);
MaterialRGBA3Calculate(&material->main_texture_blend);
MaterialRGBA3Calculate(&material->sphere_texture_blend);
MaterialRGBA3Calculate(&material->toon_texture_blend);
}
}
int RowEchelonDecompose(MatrixTemplate<T>& A,MatrixTemplate<T>& B,Real zeroTol)
{
if(!B.isEmpty())
Assert(B.m == A.m);
int m=A.m,n=A.n;
int p=B.n;
int i,j,icur=0,jcur;
T temp;
for(jcur=0;jcur<n;jcur++) {
//find pivot element in col jcur from rows icur..m
Real big=Zero;
int ipivot=-1;
for(i=icur;i<m;i++) {
if(Abs(A(i,jcur)) > big) {
ipivot = i;
big = Abs(A(i,jcur));
}
}
if(!FuzzyZero(big,zeroTol)) { //nonzero pivot found
//exchange rows ipivot,icur
if(ipivot != icur) {
for(j=jcur;j<n;j++) SWAP(A(ipivot,j),A(icur,j));
for(j=0;j<p;j++) SWAP(B(ipivot,j),B(icur,j));
}
//eliminate rows below icur
T scale;
for(i=icur+1;i<m;i++) {
//set row(ai) = row(ai)-aiJ/aIJ*row(aI)
scale = A(i,jcur)/A(icur,jcur);
for(j=jcur;j<n;j++) A(i,j) -= A(icur,j)*scale;
for(j=0;j<p;j++) B(i,j) -= B(icur,j)*scale;
A(i,jcur)=Zero;
}
icur++;
}
else {
//either zero pivot, or very small one
//set to zero to reduce numerical difficulties later
for(i=icur;i<m;i++) A(i,jcur)=Zero;
}
}
return Max(m-icur,n-jcur);
}
ConvergenceResult Root_Newton(ScalarFieldFunction& f,const Vector& x0, Vector& x, int& iters, Real tolx, Real tolf)
{
//move in gradient direction to set f to 0
//f(x) ~= f(x0) + df/dx(x0).(x-x0) + O(h^2)
//=> 0 = fx + grad.(x-x0) = fx + grad.grad*h => h = -fx/grad.grad
Real fx;
Vector grad(x.n);
if(&x != &x0) x = x0;
int maxIters=iters;
for (iters=0;iters<maxIters;iters++) {
f.PreEval(x);
fx = f.Eval(x);
f.Gradient(x,grad);
//Check function convergence.
if (Abs(fx) <= tolf) return ConvergenceF;
Real gSquared = grad.normSquared();
if(FuzzyZero(gSquared)) return LocalMinimum;
x.madd(grad,-fx/gSquared);
if (Abs(grad.maxAbsElement()*fx/gSquared) <= tolx) return ConvergenceX;
}
return MaxItersReached;
}
bool Matrix3::setInverse(const Matrix3& a)
{
Matrix3 _a(a);
Matrix3& _b = *this;
_b.setIdentity();
int i,j,i1;
for(j=0;j<3;j++) {
i1 = j;
for(i=j+1;i<3;i++)
if (Abs(_a(i,j)) > Abs(_a(i1,j)))
i1 = i;
rowswap(_a,i1,j);
rowswap(_b,i1,j);
if (FuzzyZero(_a(j,j))){
fprintf(stderr, "Taking inverse of singular Matrix3\n");
return false;
}
register Real div = Inv(_a(j,j));
_b(j,0) *= div;
_b(j,1) *= div;
_b(j,2) *= div;
_a(j,0) *= div;
_a(j,1) *= div;
_a(j,2) *= div;
for(i=0;i<3;i++) {
if (i != j) {
register Real tmp = _a(i,j);
_b(i,0) -= tmp*_b(j,0);
_b(i,1) -= tmp*_b(j,1);
_b(i,2) -= tmp*_b(j,2);
_a(i,0) -= tmp*_a(j,0);
_a(i,1) -= tmp*_a(j,1);
_a(i,2) -= tmp*_a(j,2);
}
}
}
return true;
}
bool LDLDecomposition<T>::downdate(const VectorT& _x)
{
VectorT x = _x; //make a copy, we'll change it
int n=LDL.n;
Assert(x.n == n);
T alpha=1;
for(int i=0;i<n;i++) {
T deltai = LDL(i,i);
T temp = alpha - Sqr(x(i))/deltai;
deltai = deltai*temp;
if(FuzzyZero(deltai,zeroTolerance)) return false;
T gamma = x(i)/deltai;
deltai = deltai / alpha;
alpha = temp;
LDL(i,i) = deltai;
for(int k=i+1;k<n;k++) {
x(k) -= x(i)*LDL(k,i);
LDL(k,i) -= gamma*x(k);
}
}
return true;
}
Real RotationFit(const vector<Vector3>& a,const vector<Vector3>& b,Matrix3& R)
{
Assert(a.size() == b.size());
assert(a.size() >= 3);
Matrix3 C;
C.setZero();
for(size_t k=0;k<a.size();k++) {
for(int i=0;i<3;i++)
for(int j=0;j<3;j++)
C(i,j) += a[k][j]*b[k][i];
}
//let A=[a1 ... an]^t, B=[b1 ... bn]^t
//solve for min sum of squares of E=ARt-B
//let C=AtB
//solution is given by CCt = RtCtCR
//Solve C^tR = R^tC with SVD CC^t = R^tC^tCR
//CtRX = RtCX
//C = RCtR
//Ct = RtCRt
//=> CCt = RCtCRt
//solve SVD of C and Ct (giving eigenvectors of CCt and CtC
//C = UWVt => Ct=VWUt
//=> UWUt = RVWVtRt
//=> U=RV => R=UVt
Matrix mC(3,3),mCtC(3,3);
Copy(C,mC);
SVDecomposition<Real> svd;
if(!svd.set(mC)) {
cerr<<"RotationFit: Couldn't set svd of covariance matrix"<<endl;
R.setIdentity();
return Inf;
}
Matrix mR;
mR.mulTransposeB(svd.U,svd.V);
Copy(mR,R);
if(R.determinant() < 0) { //it's a mirror
svd.sortSVs();
if(!FuzzyZero(svd.W(2),(Real)1e-2)) {
cerr<<"RotationFit: Uhh... what do we do? SVD of rotation doesn't have a zero singular value"<<endl;
/*
cerr<<svd.W<<endl;
cerr<<"Press any key to continue"<<endl;
getchar();
*/
}
//negate the last column of V
Vector vi;
svd.V.getColRef(2,vi);
vi.inplaceNegative();
mR.mulTransposeB(svd.V,svd.U);
Copy(mR,R);
Assert(R.determinant() > 0);
}
Real sum=0;
for(size_t k=0;k<a.size();k++)
sum += b[k].distanceSquared(R*a[k]);
return sum;
}
void PmxBonePerformTransform(PMX_BONE* bone)
{
float rotation[4] = IDENTITY_QUATERNION;
float position[3] = {0};
if((bone->flags & PMX_BONE_FLAG_HAS_INHERENT_ROTATION) != 0)
{
PMX_BONE *parent_bone = bone->parent_inherent_bone;
if(parent_bone != NULL)
{
if((parent_bone->flags & PMX_BONE_FLAG_HAS_INHERENT_ROTATION) != 0)
{
MultiQuaternion(rotation, parent_bone->local_inherent_rotation);
}
else
{
float rotate_value[4];
COPY_VECTOR4(rotate_value, parent_bone->local_rotation);
MultiQuaternion(rotate_value, parent_bone->local_morph_rotation);
MultiQuaternion(rotation, rotate_value);
//MultiQuaternion(rotation, parent_bone->local_morph_rotation);
//MultiQuaternion(rotation, parent_bone->local_rotation);
}
}
if(FuzzyZero(bone->coefficient - 1.0f) == 0)
{
float set_value[4] = IDENTITY_QUATERNION;
QuaternionSlerp(set_value, set_value, rotation, bone->coefficient);
COPY_VECTOR4(rotation, set_value);
}
if(parent_bone != NULL && (parent_bone->flags & PMX_BONE_FLAG_HAS_INVERSE_KINEMATICS) != 0)
{
MultiQuaternion(rotation, parent_bone->joint_rotation);
}
COPY_VECTOR4(bone->local_inherent_rotation, rotation);
MultiQuaternion(bone->local_inherent_rotation, bone->local_rotation);
MultiQuaternion(bone->local_inherent_rotation, bone->local_morph_rotation);
QuaternionNormalize(bone->local_inherent_rotation);
}
MultiQuaternion(rotation, bone->local_rotation);
MultiQuaternion(rotation, bone->local_morph_rotation);
MultiQuaternion(rotation, bone->joint_rotation);
QuaternionNormalize(rotation);
if((bone->flags & PMX_BONE_FLAG_HAS_INHERENT_TRANSLATION) != 0)
{
PMX_BONE *parent_bone = bone->parent_inherent_bone;
if(parent_bone != NULL)
{
if((parent_bone->flags & PMX_BONE_FLAG_HAS_INHERENT_TRANSLATION) != 0)
{
position[0] += parent_bone->local_inherent_translation[0];
position[1] += parent_bone->local_inherent_translation[1];
position[2] += parent_bone->local_inherent_translation[2];
}
else
{
position[0] += parent_bone->local_translation[0] + parent_bone->local_morph_translation[0];
position[1] += parent_bone->local_translation[1] + parent_bone->local_morph_translation[1];
position[2] += parent_bone->local_translation[2] + parent_bone->local_morph_translation[2];
}
}
if(FuzzyZero(bone->coefficient - 1) == 0)
{
position[0] *= bone->coefficient;
position[1] *= bone->coefficient;
position[2] *= bone->coefficient;
}
COPY_VECTOR3(bone->local_inherent_translation, position);
}
position[0] += bone->local_translation[0] + bone->local_morph_translation[0];
position[1] += bone->local_translation[1] + bone->local_morph_translation[1];
position[2] += bone->local_translation[2] + bone->local_morph_translation[2];
PmxBoneUpdateWorldTransform(bone, position, rotation);
}
void* BaseJointCreateConstraint(BASE_JOINT* joint)
{
void *ret;
int index;
int i;
switch(joint->type)
{
case JOINT_TYPE_GENERIC_6DOF_SPRING_CONSTRAINT:
{
float scalar_value;
void *constraint = BaseJointCreateGeneric6DofSpringConstraint(joint);
for(i=0; i<3; i++)
{
scalar_value = joint->position_stiffness[i];
if(!FuzzyZero(scalar_value))
{
BtGeneric6DofSpringConstraintEnableSpring(constraint, i, TRUE);
BtGeneric6DofSpringConstraintSetStiffness(constraint, i, scalar_value);
BtGeneric6DofSpringConstraintSetDamping(constraint, i, BASE_JOINT_DEFAULT_DAMPING);
}
}
for(i=0; i<3; i++)
{
scalar_value = joint->rotation_stiffness[i];
if(!FuzzyZero(scalar_value))
{
index = i + 3;
BtGeneric6DofSpringConstraintEnableSpring(constraint, index, TRUE);
BtGeneric6DofSpringConstraintSetStiffness(constraint, index, scalar_value);
BtGeneric6DofSpringConstraintSetDamping(constraint, index, BASE_JOINT_DEFAULT_DAMPING);
}
}
BtGeneric6DofSpringConstraintSetEquilibriumPoint(constraint);
ret = constraint;
}
break;
case JOINT_TYPE_GENERIC_6DOF_CONSTRAINT:
ret = BaseJointCreateGeneric6DofSpringConstraint(joint);
break;
case JOINT_TYPE_POINT2POINT_CONSTRAINT:
{
const float zero_vector[3] = {0, 0, 0};
joint->constraint_ptr = BtPoint2PointConstraintNew(
joint->rigid_body1, joint->rigid_body2, zero_vector, zero_vector);
ret = joint->constraint_ptr;
}
break;
case JOINT_TYPE_CONE_TWIST_CONSTRAINT:
{
void *world_transform;
uint8 transform_a[TRANSFORM_SIZE];
uint8 transform_b[TRANSFORM_SIZE];
const float basis[9] = {0};
int enable_motor;
world_transform = BtTransformNew(basis);
BaseJointGetWorldTransform(joint, world_transform);
BtRigidBodyGetCenterOfMassTransform(joint->rigid_body1, transform_a);
BtTransformInverse(transform_a);
BtTransformMulti(transform_a, world_transform, transform_a);
BtRigidBodyGetCenterOfMassTransform(joint->rigid_body2, transform_b);
BtTransformInverse(transform_b);
BtTransformMulti(transform_b, world_transform, transform_b);
joint->constraint_ptr = BtConeTwistConstraintNew(
joint->rigid_body1, joint->rigid_body2, transform_a, transform_b);
BtConeTwistConstraintSetLimit(joint->constraint_ptr,
joint->rotation_lower_limit[0], joint->rotation_lower_limit[1], joint->rotation_lower_limit[2],
joint->position_stiffness[0], joint->position_stiffness[1], joint->position_stiffness[2]);
BtConeTwistConstraintSetDamping(joint->constraint_ptr, joint->position_lower_limit[0]);
BtConeTwistConstraintSetFixThreshold(joint->constraint_ptr, joint->position_upper_limit[0]);
enable_motor = FuzzyZero(joint->position_lower_limit[2]);
BtConeTwistConstraintEnableMotor(joint->constraint_ptr, enable_motor);
if(enable_motor)
{
float rotation[4];
BtConeTwistConstraintSetMaxMotorImpulse(joint->constraint_ptr, joint->position_upper_limit[2]);
QuaternionSetEulerZYX(rotation, - joint->rotation_stiffness[0], - joint->rotation_stiffness[1], joint->rotation_stiffness[2]);
BtConeTwistConstraintSetMotorTargetInConstraintSpace(
joint->constraint_ptr, rotation);
}
DeleteBtTransform(world_transform);
ret = joint->constraint_ptr;
}
break;
case JOINT_TYPE_SLIDER_CONSTRAINT:
{
const float basis[9] = {0};
void *world_transform;
uint8 frame_in_a[TRANSFORM_SIZE];
uint8 frame_in_b[TRANSFORM_SIZE];
int enable_powered_motor;
world_transform = BtTransformNew(basis);
BaseJointGetWorldTransform(joint, world_transform);
BtRigidBodyGetCenterOfMassTransform(joint->rigid_body1, frame_in_a);
BtTransformInverse(frame_in_a);
BtTransformMulti(frame_in_a, world_transform, frame_in_a);
BtRigidBodyGetCenterOfMassTransform(joint->rigid_body2, frame_in_b);
BtTransformInverse(frame_in_b);
BtTransformMulti(frame_in_b, world_transform, frame_in_b);
joint->constraint_ptr = BtSliderConstraintNew(
joint->rigid_body1, joint->rigid_body2, frame_in_a, frame_in_b, TRUE);
BtSliderConstraintSetLowerLinearLimit(joint->constraint_ptr, joint->position_lower_limit[0]);
BtSliderConstraintSetUpperLinearLimit(joint->constraint_ptr, joint->position_upper_limit[0]);
BtSliderConstraintSetLowerAngularLimit(joint->constraint_ptr, joint->rotation_lower_limit[0]);
BtSliderConstraintSetUpperAngularLimit(joint->constraint_ptr, joint->rotation_upper_limit[0]);
enable_powered_motor = FuzzyZero(joint->position_stiffness[0]);
BtSliderConstraintSetPoweredLinearMotor(joint->constraint_ptr, enable_powered_motor);
if(enable_powered_motor)
{
BtSliderConstraintSetTargetLinearMotorVelocity(joint->constraint_ptr, joint->position_stiffness[1]);
BtSliderConstraintSetMaxLinearMotorForce(joint->constraint_ptr, joint->position_stiffness[2]);
}
enable_powered_motor = FuzzyZero(joint->rotation_stiffness[0]);
BtSliderConstraintSetPoweredAngluarMotor(joint->constraint_ptr, enable_powered_motor);
if(enable_powered_motor)
{
BtSliderConstraintSetTargetAngluarMotorVelocity(joint->constraint_ptr, joint->rotation_stiffness[1]);
BtSliderConstraintSetMaxAngluarMotorForce(joint->constraint_ptr, joint->rotation_stiffness[2]);
}
DeleteBtTransform(world_transform);
ret = joint->constraint_ptr;
}
break;
case JOINT_TYPE_HINGE_CONSTRAINT:
{
void *world_transform;
uint8 transform_a[TRANSFORM_SIZE];
uint8 transform_b[TRANSFORM_SIZE];
const float basis[9] = {0};
int enable_motor;
world_transform = BtTransformNew(basis);
BaseJointGetWorldTransform(joint, world_transform);
BtRigidBodyGetCenterOfMassTransform(joint->rigid_body1, transform_a);
BtTransformInverse(transform_a);
BtTransformMulti(transform_a, world_transform, transform_a);
BtRigidBodyGetCenterOfMassTransform(joint->rigid_body2, transform_b);
BtTransformInverse(transform_b);
BtTransformMulti(transform_b, world_transform, transform_b);
joint->constraint_ptr = BtHingeConstraintNew(joint->rigid_body1, joint->rigid_body2, transform_a, transform_b);
BtHingeConstraintSetLimit(joint->constraint_ptr, joint->rotation_lower_limit[0], joint->rotation_upper_limit[1],
joint->position_stiffness[0], joint->position_stiffness[1], joint->position_stiffness[2]);
enable_motor = FuzzyZero(joint->rotation_stiffness[2]);
BtHingeConstraintEnableMotor(joint->constraint_ptr, enable_motor);
if(enable_motor)
{
BtHingeConstraintEnableAngularMotor(joint->constraint_ptr, enable_motor,
joint->rotation_stiffness[1], joint->rotation_stiffness[2]);
}
DeleteBtTransform(world_transform);
ret = joint->constraint;
}
break;
default:
ret = NULL;
}
if(ret != NULL)
{
BtTypedConstraintSetUserConstraintPointer(ret, (void*)joint);
}
return ret;
}
void ClusterContacts(vector<dContactGeom>& contacts,int maxClusters,Real clusterNormalScale)
{
if((int)contacts.size() <= maxClusters) return;
vector<Vector> pts(contacts.size());
for(size_t i=0;i<pts.size();i++) {
pts[i].resize(7);
pts[i][0] = contacts[i].pos[0];
pts[i][1] = contacts[i].pos[1];
pts[i][2] = contacts[i].pos[2];
pts[i][3] = contacts[i].normal[0]*clusterNormalScale;
pts[i][4] = contacts[i].normal[1]*clusterNormalScale;
pts[i][5] = contacts[i].normal[2]*clusterNormalScale;
pts[i][6] = contacts[i].depth;
}
Statistics::KMeans kmeans(pts,maxClusters);
//randomized
//kmeans.RandomInitialCenters();
//deterministic
for(size_t i=0;i<kmeans.centers.size();i++)
kmeans.centers[i] = kmeans.data[(i*pts.size())/kmeans.centers.size()];
int iters=20;
kmeans.Iterate(iters);
contacts.resize(kmeans.centers.size());
vector<int> degenerate;
for(size_t i=0;i<contacts.size();i++) {
contacts[i].pos[0] = kmeans.centers[i][0];
contacts[i].pos[1] = kmeans.centers[i][1];
contacts[i].pos[2] = kmeans.centers[i][2];
contacts[i].normal[0] = kmeans.centers[i][3]/clusterNormalScale;
contacts[i].normal[1] = kmeans.centers[i][4]/clusterNormalScale;
contacts[i].normal[2] = kmeans.centers[i][5]/clusterNormalScale;
Real len = Vector3(contacts[i].normal[0],contacts[i].normal[1],contacts[i].normal[2]).length();
if(FuzzyZero(len) || !IsFinite(len)) {
printf("ODESimulator: Warning, clustered normal became zero/infinite\n");
//pick any in the cluster
int found = -1;
for(size_t k=0;k<kmeans.labels.size();k++) {
if(kmeans.labels[k] == (int)i) {
found = (int)k;
break;
}
}
if(found < 0) {
//strange -- degenerate cluster?
degenerate.push_back(i);
continue;
}
contacts[i].pos[0] = pts[found][0];
contacts[i].pos[1] = pts[found][1];
contacts[i].pos[2] = pts[found][2];
contacts[i].normal[0] = pts[found][3];
contacts[i].normal[1] = pts[found][4];
contacts[i].normal[2] = pts[found][5];
Real len = Vector3(contacts[i].normal[0],contacts[i].normal[1],contacts[i].normal[2]).length();
contacts[i].normal[0] /= len;
contacts[i].normal[1] /= len;
contacts[i].normal[2] /= len;
contacts[i].depth = pts[found][6];
continue;
}
contacts[i].normal[0] /= len;
contacts[i].normal[1] /= len;
contacts[i].normal[2] /= len;
//cout<<"Clustered contact "<<contacts[i].pos[0]<<" "<<contacts[i].pos[1]<<" "<<contacts[i].pos[2]<<endl;
//cout<<"Clustered normal "<<contacts[i].normal[0]<<" "<<contacts[i].normal[1]<<" "<<contacts[i].normal[2]<<endl;
contacts[i].depth = kmeans.centers[i][6];
}
reverse(degenerate.begin(),degenerate.end());
for(size_t i=0;i<degenerate.size();i++) {
contacts.erase(contacts.begin()+degenerate[i]);
}
}
void PmxBoneSolveInverseKinematics(PMX_BONE* bone)
{
PMX_IK_CONSTRAINT *constraints = (PMX_IK_CONSTRAINT*)bone->constraints->buffer;
float root_bone_position[3];
const float angle_limit = bone->angle_limit;
const int num_constraints = (int)bone->constraints->num_data;
const int num_iteration = bone->num_iteration;
const int num_half_of_iteration = num_iteration / 2;
PMX_BONE *effector_bone = (PMX_BONE*)bone->interface_data.effector_bone;
float original_target_rotation[4];
float joint_rotation[4] = IDENTITY_QUATERNION;
float new_joint_local_rotation[4];
float matrix[9];
float local_effector_position[3] = {0};
float local_root_bone_position[3] = {0};
float local_axis[3] = {0};
int i, j, k;
if((bone->flags & PMX_BONE_FLAG_HAS_INVERSE_KINEMATICS) == 0)
{
return;
}
BtTransformGetOrigin(bone->world_transform, root_bone_position);
COPY_VECTOR4(original_target_rotation, effector_bone->local_rotation);
for(i=0; i<num_iteration; i++)
{
int perform_constraint = i < num_half_of_iteration;
for(j=0; j<num_constraints; j++)
{
PMX_IK_CONSTRAINT *constraint = &constraints[j];
PMX_BONE *joint_bone = constraint->joint_bone;
float current_effector_position[3];
void *joint_bone_transform = BtTransformCopy(joint_bone->world_transform);
void *inversed_joint_bone_transform = BtTransformCopy(joint_bone->world_transform);
float dot;
float new_angle_limit;
float angle;
float value;
BtTransformGetOrigin(effector_bone->world_transform, current_effector_position);
BtTransformInverse(inversed_joint_bone_transform);
BtTransformMultiVector3(inversed_joint_bone_transform, root_bone_position, local_root_bone_position);
Normalize3DVector(local_root_bone_position);
BtTransformMultiVector3(inversed_joint_bone_transform, current_effector_position, local_effector_position);
Normalize3DVector(local_effector_position);
dot = Dot3DVector(local_root_bone_position, local_effector_position);
if(FuzzyZero(dot) != 0)
{
break;
}
Cross3DVector(local_axis, local_effector_position, local_root_bone_position);
SafeNormalize3DVector(local_axis);
new_angle_limit = angle_limit * (j + 1) * 2;
/*if(dot < -1)
{
angle = -1;
}
else if(dot > 1)
{
angle = 1;
}
else
{
angle = acosf(dot);
}*/
value = dot;
value = ExtendedFuzzyZero(1.0f - value) ? 1.0f : ExtendedFuzzyZero(1.0f + value) ? -1.0f : value;
angle = acosf(value);
CLAMPED(angle, - new_angle_limit, new_angle_limit);
QuaternionSetRotation(joint_rotation, local_axis, angle);
if(constraint->has_angle_limit != FALSE && perform_constraint != FALSE)
{
float lower_limit[3];
float upper_limit[3];
COPY_VECTOR3(lower_limit, constraint->lower_limit);
COPY_VECTOR3(upper_limit, constraint->upper_limit);
if(i == 0)
{
if(FuzzyZero(lower_limit[1]) && FuzzyZero(upper_limit[1])
&& FuzzyZero(lower_limit[2]) && FuzzyZero(upper_limit[2]))
{
local_axis[0] = 1;
local_axis[1] = 0;
local_axis[2] = 0;
}
else if(FuzzyZero(lower_limit[0]) && FuzzyZero(upper_limit[0])
&& FuzzyZero(lower_limit[2]) && FuzzyZero(upper_limit[2]))
{
local_axis[0] = 0;
local_axis[1] = 1;
local_axis[2] = 0;
}
else if(FuzzyZero(lower_limit[0]) && FuzzyZero(upper_limit[0])
&& FuzzyZero(lower_limit[1]) && FuzzyZero(upper_limit[1]))
{
local_axis[0] = 0;
local_axis[1] = 0;
local_axis[2] = 1;
}
QuaternionSetRotation(joint_rotation, local_axis, angle);
}
else
{
float x1, y1, z1, x2, y2, z2, x3, y3, z3;
Matrix3x3SetRotation(matrix, joint_rotation);
Matrix3x3GetEulerZYX(matrix, &z1, &y1, &x1);
Matrix3x3SetRotation(matrix, joint_bone->local_rotation);
Matrix3x3GetEulerZYX(matrix, &z2, &y2, &x2);
x3 = x1 + x2, y3 = y1 + y2, z3 = z1 + z2;
x1 = ClampAngle(lower_limit[0], upper_limit[0], x3, x1);
y1 = ClampAngle(lower_limit[1], upper_limit[1], y3, y1);
z1 = ClampAngle(lower_limit[2], upper_limit[2], z3, z1);
QuaternionSetEulerZYX(joint_rotation, z1, y1, x1);
}
COPY_VECTOR4(new_joint_local_rotation, joint_rotation);
MultiQuaternion(new_joint_local_rotation, joint_bone->local_rotation);
}
else if(i == 0)
{
//COPY_VECTOR4(new_joint_local_rotation, joint_bone->local_rotation);
//MultiQuaternion(new_joint_local_rotation, joint_rotation);
COPY_VECTOR4(new_joint_local_rotation, joint_rotation);
MultiQuaternion(new_joint_local_rotation, joint_bone->local_rotation);
}
else
{
//COPY_VECTOR4(new_joint_local_rotation, joint_rotation);
//MultiQuaternion(new_joint_local_rotation, joint_bone->local_rotation);
COPY_VECTOR4(new_joint_local_rotation, joint_bone->local_rotation);
MultiQuaternion(new_joint_local_rotation, joint_rotation);
}
PmxBoneSetLocalOrientation(joint_bone, new_joint_local_rotation);
COPY_VECTOR4(joint_bone->joint_rotation, joint_rotation);
for(k=j; k>=0; k--)
{
PMX_IK_CONSTRAINT *ik = &constraints[k];
PMX_BONE *joint = ik->joint_bone;
PmxBoneUpdateWorldTransformSimple(joint);
}
PmxBoneUpdateWorldTransformSimple(effector_bone);
DeleteBtTransform(joint_bone_transform);
DeleteBtTransform(inversed_joint_bone_transform);
}
}
PmxBoneSetLocalOrientation(effector_bone, original_target_rotation);
}
