void CIKLimb::Calculate(CKinematics* K,const Fmatrix &obj)
{
SCalculateData cd(this,K,obj);
m_prev_state_anim=true;
Collide(&cd);
if(cd.m_tri)
{
Matrix m ;
GoalMatrix (m,&cd) ;
#ifdef DEBUG
if(m_limb.SetGoal(m,ph_dbg_draw_mask.test(phDbgIKLimits)))
#else
if(m_limb.SetGoal(m,TRUE))
#endif
{
Fmatrix hip;
CBoneData& BD=K->LL_GetData(m_bones[0]);
hip.mul_43(K->LL_GetTransform(BD.GetParentID()),BD.bind_transform);
hip.invert();
float x[7];
Fvector pos;
pos.set(K->LL_GetTransform(m_bones[1]).c);
hip.transform_tiny(pos);
xm2im.transform_tiny(pos);
if(m_limb.SolveByPos(cast_fp(pos),x))
{
cd.m_angles=x;
CalculateBones(&cd);
m_prev_state_anim=false;
}
}
}
}
void CKinematics::DebugRender(Fmatrix& XFORM)
{
CalculateBones ();
CBoneData::BoneDebug dbgLines;
(*bones)[iRoot]->DebugQuery (dbgLines);
Fvector Z; Z.set(0,0,0);
Fvector H1; H1.set(0.01f,0.01f,0.01f);
Fvector H2; H2.mul(H1,2);
for (u32 i=0; i<dbgLines.size(); i+=2) {
Fmatrix& M1 = bone_instances[dbgLines[i]].mTransform;
Fmatrix& M2 = bone_instances[dbgLines[i+1]].mTransform;
Fvector P1,P2;
M1.transform_tiny(P1,Z);
M2.transform_tiny(P2,Z);
RCache.dbg_DrawLINE(XFORM,P1,P2,D3DCOLOR_XRGB(0,255,0));
Fmatrix M;
M.mul_43(XFORM,M2);
RCache.dbg_DrawOBB(M,H1,D3DCOLOR_XRGB(255,255,255));
RCache.dbg_DrawOBB(M,H2,D3DCOLOR_XRGB(255,255,255));
}
for (u32 b=0; b<bones->size(); b++)
{
Fobb& obb = (*bones)[b]->obb;
Fmatrix& Mbone = bone_instances[b].mTransform;
Fmatrix Mbox; obb.xform_get(Mbox);
Fmatrix X; X.mul(Mbone,Mbox);
Fmatrix W; W.mul(XFORM,X);
RCache.dbg_DrawOBB(W,obb.m_halfsize,D3DCOLOR_XRGB(0,0,255));
}
}
void CIKLimb::BonesCallback2 (CBoneInstance* B)
{
SCalculateData* D=(SCalculateData*)B->Callback_Param;
CIKLimb* L=D->m_limb ;
float const *x=D->m_angles ;
{
Fmatrix bm;//=B->mTransform ;
//tmp_pos .set(bm.c) ;
// Fvector ra;B->mTransform.getXYZ(ra) ;
// bm .setXYZ(-x[4],-x[5],-x[6]) ;
Matrix tbm;
float t[3];
t[0] = x[6]; t[1] = x[5]; t[2] = x[4] ;
EulerEval(ZXY,t,tbm) ;
IM2XM(tbm,bm) ;
CKinematics *K=D->m_K;
CBoneData& BD=K->LL_GetData(L->m_bones[2]) ;
Fmatrix start;start.mul_43(K->LL_GetTransform(BD.GetParentID()),BD.bind_transform);
Fmatrix inv_start;inv_start.invert(start);
Fmatrix dif;dif.mul_43(inv_start,B->mTransform);
Fmatrix ikdif;
Fvector a;
dif.getXYZ(a);
XM2IM(dif,ikdif);
//B->mTransform.mul_43(K->LL_GetTransform(BD.GetParentID()),BD.bind_transform);
//B->mTransform.mulB_43(bm) ;
B->mTransform.mul_43(start,bm);
#ifdef DEBUG
if(ph_dbg_draw_mask.test(phDbgDrawIKGoal))
{
Fmatrix DBGG;
DBGG.mul_43(*D->m_obj,B->mTransform);
DBG_DrawMatrix(DBGG,0.3f);
DBGG.mul_43(*D->m_obj,start);
DBG_DrawMatrix(DBGG,0.3f);
}
#endif
//bm.c .set(tmp_pos) ;
}
}
void CIKLimb::BonesCallback0 (CBoneInstance* B)
{
SCalculateData* D=(SCalculateData*)B->Callback_Param;
CIKLimb* L=D->m_limb ;
float const *x=D->m_angles ;
Fmatrix bm;
//tmp_pos .set(bm.c) ;
//Fvector ra;B->mTransform.getXYZ(ra) ;
Matrix tbm;
float t[3];
t[0] = x[2]; t[1] = x[1]; t[2] = x[0] ;
EulerEval(ZXY,t,tbm) ;
IM2XM(tbm,bm) ;
Fmatrix cmp_save;cmp_save.set(B->mTransform) ;
CKinematics *K=D->m_K;
CBoneData& BD=K->LL_GetData(L->m_bones[0]);
B->mTransform.mul_43(K->LL_GetTransform(BD.GetParentID()),BD.bind_transform);
#ifdef DEBUG
if(ph_dbg_draw_mask.test(phDbgDrawIKGoal))
{
Fmatrix DBGG;
DBGG.mul_43(*D->m_obj,B->mTransform);
DBG_DrawMatrix(DBGG,0.3f);
}
#endif
B->mTransform.mulB_43(bm) ;
#ifdef DEBUG
if(ph_dbg_draw_mask.test(phDbgDrawIKGoal))
{
Fmatrix DBGG;
DBGG.mul_43(*D->m_obj,B->mTransform);
DBG_DrawMatrix(DBGG,0.3f);
}
#endif
Fmatrix cmp_savei;cmp_savei.invert(cmp_save);
Fmatrix dif;dif.mul_43(cmp_savei,B->mTransform);
}
IC bool cmp( const Fmatrix &f0, const Fmatrix &f1 )
{
Fmatrix if0; if0.invert( f0 );
Fmatrix cm;cm.mul_43( if0, f1 );
Fvector ax; float ang;
Fquaternion q;
q.set( cm );
q.get_axis_angle( ax, ang );
return ang < cmp_angle && cm.c.square_magnitude() < cmp_ldisp* cmp_ldisp ;
}
Fvector CScriptGameObject::bone_position (LPCSTR bone_name) const
{
u16 bone_id;
if (xr_strlen(bone_name))
bone_id = smart_cast<IKinematics*>(object().Visual())->LL_BoneID(bone_name);
else
bone_id = smart_cast<IKinematics*>(object().Visual())->LL_GetBoneRoot();
Fmatrix matrix;
matrix.mul_43 (object().XFORM(),smart_cast<IKinematics*>(object().Visual())->LL_GetBoneInstance(bone_id).mTransform);
return (matrix.c);
}
void CSkeletonX::_Render (ref_geom& hGeom, u32 vCount, u32 iOffset, u32 pCount)
{
RCache.stat.r.s_dynamic.add (vCount);
switch (RenderMode)
{
case RM_SKINNING_SOFT:
_Render_soft (hGeom,vCount,iOffset,pCount);
RCache.stat.r.s_dynamic_sw.add (vCount);
break;
case RM_SINGLE:
{
Fmatrix W; W.mul_43 (RCache.xforms.m_w,Parent->LL_GetTransform_R (u16(RMS_boneid)));
RCache.set_xform_world (W);
RCache.set_Geometry (hGeom);
RCache.Render (D3DPT_TRIANGLELIST,0,0,vCount,iOffset,pCount);
RCache.stat.r.s_dynamic_inst.add (vCount);
}
break;
case RM_SKINNING_1B:
case RM_SKINNING_2B:
case RM_SKINNING_3B:
case RM_SKINNING_4B:
{
// transfer matrices
ref_constant array = RCache.get_c(s_bones_array_const);
u32 count = RMS_bonecount;
for (u32 mid = 0; mid<count; mid++)
{
Fmatrix& M = Parent->LL_GetTransform_R(u16(mid));
u32 id = mid*3;
RCache.set_ca (&*array,id+0,M._11,M._21,M._31,M._41);
RCache.set_ca (&*array,id+1,M._12,M._22,M._32,M._42);
RCache.set_ca (&*array,id+2,M._13,M._23,M._33,M._43);
}
// render
RCache.set_Geometry (hGeom);
RCache.Render (D3DPT_TRIANGLELIST,0,0,vCount,iOffset,pCount);
if (RM_SKINNING_1B==RenderMode)
RCache.stat.r.s_dynamic_1B.add (vCount);
else
if (RM_SKINNING_2B==RenderMode)
RCache.stat.r.s_dynamic_2B.add (vCount);
else
if (RM_SKINNING_3B==RenderMode)
RCache.stat.r.s_dynamic_3B.add (vCount);
else
if (RM_SKINNING_4B==RenderMode)
RCache.stat.r.s_dynamic_4B.add (vCount);
}
break;
}
}
bool CSpaceRestrictorWrapper::inside (const Fvector &position, float radius) const
{
Fsphere sphere;
sphere.P = position;
sphere.R = radius;
typedef CShapeData::ShapeVec ShapeVec;
ShapeVec::const_iterator I = object().shapes.begin();
ShapeVec::const_iterator E = object().shapes.end();
for ( ; I != E; ++I) {
switch ((*I).type) {
case 0 : {
Fsphere temp;
m_xform.transform_tiny(temp.P,(*I).data.sphere.P);
temp.R = (*I).data.sphere.R;
if (sphere.intersect(temp))
return (true);
continue;
}
case 1 : {
Fmatrix temp;
temp.mul_43 (m_xform,(*I).data.box);
// Build points
Fvector vertices;
Fvector points[8];
Fplane plane;
vertices.set(-.5f, -.5f, -.5f); temp.transform_tiny(points[0],vertices);
vertices.set(-.5f, -.5f, +.5f); temp.transform_tiny(points[1],vertices);
vertices.set(-.5f, +.5f, +.5f); temp.transform_tiny(points[2],vertices);
vertices.set(-.5f, +.5f, -.5f); temp.transform_tiny(points[3],vertices);
vertices.set(+.5f, +.5f, +.5f); temp.transform_tiny(points[4],vertices);
vertices.set(+.5f, +.5f, -.5f); temp.transform_tiny(points[5],vertices);
vertices.set(+.5f, -.5f, +.5f); temp.transform_tiny(points[6],vertices);
vertices.set(+.5f, -.5f, -.5f); temp.transform_tiny(points[7],vertices);
plane.build(points[0],points[3],points[5]); if (plane.classify(sphere.P)>sphere.R) break;
plane.build(points[1],points[2],points[3]); if (plane.classify(sphere.P)>sphere.R) break;
plane.build(points[6],points[5],points[4]); if (plane.classify(sphere.P)>sphere.R) break;
plane.build(points[4],points[2],points[1]); if (plane.classify(sphere.P)>sphere.R) break;
plane.build(points[3],points[2],points[4]); if (plane.classify(sphere.P)>sphere.R) break;
plane.build(points[1],points[0],points[6]); if (plane.classify(sphere.P)>sphere.R) break;
return (true);
}
default : NODEFAULT;
}
}
return (false);
}
void animation_movement_controller::RootBoneCallback( CBoneInstance* B )
{
VERIFY( B );
VERIFY( B->Callback_Param );
animation_movement_controller* O=( animation_movement_controller* )( B->Callback_Param );
if(O->m_control_blend->playing)
{
Fmatrix m;m.mul_43( B->mTransform, Fmatrix( ).invert( O->m_startRootXform ) );
O->m_pObjXForm.mul_43( O->m_startObjXForm, m );
}
B->mTransform.set(O->m_startRootXform);
}
void CSpaceRestrictionShape::fill_shape (const CCF_Shape::shape_def &shape)
{
Fvector start,dest;
switch (shape.type) {
case 0 : {
start.sub (Fvector().set(shape.data.sphere.P),Fvector().set(shape.data.sphere.R,0.f,shape.data.sphere.R));
dest.add (Fvector().set(shape.data.sphere.P),Fvector().set(shape.data.sphere.R,0.f,shape.data.sphere.R));
start.add (m_restrictor->Position());
dest.add (m_restrictor->Position());
break;
}
case 1 : {
Fvector points[8] = {
Fvector().set(-.5f,-.5f,-.5f),
Fvector().set(-.5f,-.5f,+.5f),
Fvector().set(-.5f,+.5f,-.5f),
Fvector().set(-.5f,+.5f,+.5f),
Fvector().set(+.5f,-.5f,-.5f),
Fvector().set(+.5f,-.5f,+.5f),
Fvector().set(+.5f,+.5f,-.5f),
Fvector().set(+.5f,+.5f,+.5f)
};
start = Fvector().set(flt_max,flt_max,flt_max);
dest = Fvector().set(flt_min,flt_min,flt_min);
Fmatrix Q;
Q.mul_43 (m_restrictor->XFORM(),shape.data.box);
Fvector temp;
for (int i=0; i<8; ++i) {
Q.transform_tiny (temp,points[i]);
start.x = _min(start.x,temp.x);
start.y = _min(start.y,temp.y);
start.z = _min(start.z,temp.z);
dest.x = _max(dest.x,temp.x);
dest.y = _max(dest.y,temp.y);
dest.z = _max(dest.z,temp.z);
}
break;
}
default : NODEFAULT;
}
ai().level_graph().iterate_vertices(start,dest,CBorderMergePredicate(this));
#ifdef DEBUG
ai().level_graph().iterate_vertices(start,dest,CShapeTestPredicate(this));
#endif
}
bool object::inside (Fvector const &position) const
{
CCF_Shape *shape = static_cast<CCF_Shape*>(collidable.model);
VERIFY (shape);
typedef xr_vector<CCF_Shape::shape_def> Shapes;
Shapes::const_iterator i = shape->shapes.begin();
Shapes::const_iterator e = shape->shapes.end();
for ( ; i != e; ++i) {
switch ((*i).type) {
case 0 : {
if ((*i).data.sphere.P.distance_to(position) <= (*i).data.sphere.R)
return (true);
continue;
}
case 1 : {
Fmatrix matrix;
const Fmatrix &box = (*i).data.box;
matrix.mul_43 (XFORM(),box);
Fvector A,B[8];
Fplane plane;
A.set (-.5f, -.5f, -.5f); matrix.transform_tiny(B[0],A);
A.set (-.5f, -.5f, +.5f); matrix.transform_tiny(B[1],A);
A.set (-.5f, +.5f, +.5f); matrix.transform_tiny(B[2],A);
A.set (-.5f, +.5f, -.5f); matrix.transform_tiny(B[3],A);
A.set (+.5f, +.5f, +.5f); matrix.transform_tiny(B[4],A);
A.set (+.5f, +.5f, -.5f); matrix.transform_tiny(B[5],A);
A.set (+.5f, -.5f, +.5f); matrix.transform_tiny(B[6],A);
A.set (+.5f, -.5f, -.5f); matrix.transform_tiny(B[7],A);
plane.build (B[0],B[3],B[5]); if (plane.classify(position) <= 0.f) return (true);
plane.build (B[1],B[2],B[3]); if (plane.classify(position) <= 0.f) return (true);
plane.build (B[6],B[5],B[4]); if (plane.classify(position) <= 0.f) return (true);
plane.build (B[4],B[2],B[1]); if (plane.classify(position) <= 0.f) return (true);
plane.build (B[3],B[2],B[4]); if (plane.classify(position) <= 0.f) return (true);
plane.build (B[1],B[0],B[6]); if (plane.classify(position) <= 0.f) return (true);
continue;
}
default : NODEFAULT;
}
}
return (false);
}
bool CBone::Pick(float& dist, const Fvector& S, const Fvector& D, const Fmatrix& parent)
{
Fvector start, dir;
Fmatrix M; M.mul_43(parent,_LTransform());
M.invert();
M.transform_tiny(start,S);
M.transform_dir(dir,D);
switch (shape.type){
case SBoneShape::stBox: return shape.box.intersect (start,dir,dist);
case SBoneShape::stSphere: return shape.sphere.intersect (start,dir,dist);
case SBoneShape::stCylinder:return shape.cylinder.intersect (start,dir,dist);
default:
Fsphere S;
S.P.set(0,0,0);
S.R=0.025f;
return S.intersect(start,dir,dist);
}
}
void CSpaceRestrictorWrapper::fill_shape (const CShapeData::shape_def &shape)
{
Fvector start,dest;
switch (shape.type) {
case 0 : {
start.sub (Fvector().set(shape.data.sphere.P),Fvector().set(shape.data.sphere.R,0.f,shape.data.sphere.R));
dest.add (Fvector().set(shape.data.sphere.P),Fvector().set(shape.data.sphere.R,0.f,shape.data.sphere.R));
start.add (object().o_Position);
dest.add (object().o_Position);
break;
}
case 1 : {
Fvector points[8] = {
Fvector().set(-.5f,-.5f,-.5f),
Fvector().set(-.5f,-.5f,+.5f),
Fvector().set(-.5f,+.5f,-.5f),
Fvector().set(-.5f,+.5f,+.5f),
Fvector().set(+.5f,-.5f,-.5f),
Fvector().set(+.5f,-.5f,+.5f),
Fvector().set(+.5f,+.5f,-.5f),
Fvector().set(+.5f,+.5f,+.5f)
};
start = Fvector().set(flt_max,flt_max,flt_max);
dest = Fvector().set(flt_min,flt_min,flt_min);
Fmatrix Q;
Q.mul_43 (m_xform,shape.data.box);
Fvector temp;
for (int i=0; i<8; ++i) {
Q.transform_tiny (temp,points[i]);
start.x = _min(start.x,temp.x);
start.y = _min(start.y,temp.y);
start.z = _min(start.z,temp.z);
dest.x = _max(dest.x,temp.x);
dest.y = _max(dest.y,temp.y);
dest.z = _max(dest.z,temp.z);
}
break;
}
default : NODEFAULT;
}
level_graph().iterate_vertices (start,dest,border_merge_predicate(this,m_level_graph));
}
void Vision::o_trace (Fvector& P, float dt, float vis_threshold) {
RQR.r_clear ();
xr_vector<feel_visible_Item>::iterator I=feel_visible.begin(),E=feel_visible.end();
for (; I!=E; I++){
if (0==I->O->CFORM()) { I->fuzzy = -1; continue; }
// verify relation
if (positive(I->fuzzy) && I->O->Position().similar(I->cp_LR_dst,lr_granularity) && P.similar(I->cp_LR_src,lr_granularity))
continue;
I->cp_LR_dst = I->O->Position();
I->cp_LR_src = P;
// Fetch data
Fvector OP;
Fmatrix mE;
const Fbox& B = I->O->CFORM()->getBBox();
const Fmatrix& M = I->O->XFORM();
// Build OBB + Ellipse and X-form point
Fvector c,r;
Fmatrix T,mR,mS;
B.getcenter (c);
B.getradius (r);
T.translate (c);
mR.mul_43 (M,T);
mS.scale (r);
mE.mul_43 (mR,mS);
mE.transform_tiny (OP,I->cp_LP);
I->cp_LAST = OP;
//
Fvector D;
D.sub (OP,P);
float f = D.magnitude();
if (f>fuzzy_guaranteed){
D.div (f);
// setup ray defs & feel params
collide::ray_defs RD (P,D,f,CDB::OPT_CULL,collide::rq_target(collide::rqtStatic|collide::rqtObstacle));
SFeelParam feel_params (this,&*I,vis_threshold);
// check cache
if (I->Cache.result&&I->Cache.similar(P,D,f)){
// similar with previous query
feel_params.vis = I->Cache_vis;
// Log("cache 0");
}else{
float _u,_v,_range;
if (CDB::TestRayTri(P,D,I->Cache.verts,_u,_v,_range,false)&&(_range>0 && _range<f)) {
feel_params.vis = 0.f;
// Log("cache 1");
}else{
// cache outdated. real query.
VERIFY(!fis_zero(RD.dir.square_magnitude()));
if (g_pGameLevel->ObjectSpace.RayQuery (RQR, RD, feel_vision_callback, &feel_params, NULL, NULL)) {
I->Cache_vis = feel_params.vis ;
I->Cache.set (P,D,f,TRUE ) ;
}else{
I->Cache.set (P,D,f,FALSE) ;
}
// Log("query");
}
}
// Log("Vis",feel_params.vis);
if (feel_params.vis<feel_params.vis_threshold){
// INVISIBLE, choose next point
I->fuzzy -= fuzzy_update_novis*dt;
clamp (I->fuzzy,-.5f,1.f);
I->cp_LP.random_dir ();
I->cp_LP.mul (.7f);
}else{
// VISIBLE
I->fuzzy += fuzzy_update_vis*dt;
clamp (I->fuzzy,-.5f,1.f);
}
}
else {
// VISIBLE, 'cause near
I->fuzzy += fuzzy_update_vis*dt;
clamp (I->fuzzy,-.5f,1.f);
}
}
}
void IM_XM(const Matrix &IM,Fmatrix &XM)
{
XM.mul_43(xm2im,*((Fmatrix*)(&IM)));
}
void XM_IM(const Fmatrix &XM,Fmatrix &IM)
{
IM.mul_43(xm2im,XM);
}
void render_box (IRenderVisual *visual, const Fmatrix &xform, const Fvector &additional, bool draw_child_boxes, const u32 &color)
{
CDebugRenderer &renderer = Level().debug_renderer();
IKinematics *kinematics = smart_cast<IKinematics*>(visual);
VERIFY (kinematics);
u16 bone_count = kinematics->LL_BoneCount();
VERIFY (bone_count);
u16 visible_bone_count = kinematics->LL_VisibleBoneCount();
if (!visible_bone_count)
return;
Fmatrix matrix;
Fvector *points = (Fvector*)_alloca(visible_bone_count*8*sizeof(Fvector));
Fvector *I = points;
for (u16 i=0; i<bone_count; ++i) {
if (!kinematics->LL_GetBoneVisible(i))
continue;
const Fobb &obb = kinematics->LL_GetData(i).obb;
if (fis_zero(obb.m_halfsize.square_magnitude())) {
VERIFY (visible_bone_count > 1);
--visible_bone_count;
continue;
}
Fmatrix Mbox;
obb.xform_get (Mbox);
const Fmatrix &Mbone = kinematics->LL_GetBoneInstance(i).mTransform;
Fmatrix X;
matrix.mul_43 (xform,X.mul_43(Mbone,Mbox));
Fvector half_size = Fvector().add(obb.m_halfsize,additional);
matrix.mulB_43 (Fmatrix().scale(half_size));
if (draw_child_boxes)
renderer.draw_obb (matrix,color);
static const Fvector local_points[8] = {
Fvector().set(-1.f,-1.f,-1.f),
Fvector().set(-1.f,-1.f,+1.f),
Fvector().set(-1.f,+1.f,+1.f),
Fvector().set(-1.f,+1.f,-1.f),
Fvector().set(+1.f,+1.f,+1.f),
Fvector().set(+1.f,+1.f,-1.f),
Fvector().set(+1.f,-1.f,+1.f),
Fvector().set(+1.f,-1.f,-1.f)
};
for (u32 i=0; i<8; ++i, ++I)
matrix.transform_tiny (*I,local_points[i]);
}
VERIFY (visible_bone_count);
if (visible_bone_count == 1) {
renderer.draw_obb (matrix,color);
return;
}
VERIFY ((I - points) == (visible_bone_count*8));
MagicBox3 box = MagicMinBox(visible_bone_count*8,points);
box.ComputeVertices (points);
Fmatrix result;
result.identity ();
result.c = box.Center();
result.i.sub(points[3],points[2]).normalize();
result.j.sub(points[2],points[1]).normalize();
result.k.sub(points[2],points[6]).normalize();
Fvector scale;
scale.x = points[3].distance_to(points[2])*.5f;
scale.y = points[2].distance_to(points[1])*.5f;
scale.z = points[2].distance_to(points[6])*.5f;
result.mulB_43 (Fmatrix().scale(scale));
renderer.draw_obb (result,color);
}
void character_hit_animation_controller::GetBaseMatrix( Fmatrix &m,CEntityAlive &ea)const
{
IKinematics* CA = smart_cast<IKinematics*>(ea.Visual());
m.mul_43(ea.XFORM(),CA->LL_GetTransform(base_bone));
}
void CIKLimb::GoalMatrix(Matrix &M,SCalculateData* cd)
{
VERIFY(cd->m_tri&&cd->m_tri_hight!=-dInfinity);
const Fmatrix &obj=*cd->m_obj;
CDB::TRI *tri=cd->m_tri;
CKinematics *K=cd->m_K;
Fvector* pVerts = Level().ObjectSpace.GetStaticVerts();
Fvector normal;
normal.mknormal (pVerts[tri->verts[0]],pVerts[tri->verts[1]],pVerts[tri->verts[2]]);
VERIFY(!fis_zero(normal.magnitude()));
Fmatrix iobj;iobj.invert(obj);iobj.transform_dir(normal);
Fmatrix xm;xm.set(K->LL_GetTransform(m_bones[2]));
//Fvector dbg;
//dbg.set(Fvector().mul(normal,normal.y*tri_hight
// -normal.dotproduct(xm.i)*m_toe_position.x
// -normal.dotproduct(xm.j)*m_toe_position.y
// -normal.dotproduct(xm.k)*m_toe_position.z-m_toe_position.x
// ));
normal.invert();
Fvector ax;ax.crossproduct(normal,xm.i);
float s=ax.magnitude();
if(!fis_zero(s))
{
ax.mul(1.f/s);
xm.mulA_43(Fmatrix().rotation(ax,asinf(-s)));
}
Fvector otri;iobj.transform_tiny(otri,pVerts[tri->verts[0]]);
float tp=normal.dotproduct(otri);
Fvector add;
add.set(Fvector().mul(normal,-m_toe_position.x+tp-xm.c.dotproduct(normal)));
xm.c.add(add);
Fmatrix H;
CBoneData& bd=K->LL_GetData(m_bones[0]);
H.set(bd.bind_transform);
H.mulA_43(K->LL_GetTransform(bd.GetParentID()));
H.c.set(K->LL_GetTransform(m_bones[0]).c);
#ifdef DEBUG
if(ph_dbg_draw_mask.test(phDbgIKAnimGoalOnly)) xm.set(K->LL_GetTransform(m_bones[2]));
if(ph_dbg_draw_mask.test(phDbgDrawIKGoal))
{
Fmatrix DBGG;
DBGG.mul_43(obj,xm);
DBG_DrawMatrix(DBGG,0.2f);
DBGG.mul_43(obj,H);
DBG_DrawMatrix(DBGG,0.2f);
}
#endif
H.invert();
Fmatrix G;
G.mul_43(H,xm);
XM2IM(G,M);
}