//Очищение матрицы состояния
//Параметры: dword <номер объекта>
:void ClearModelStatusMatrix(dword objnum)
{
 ESI=Object[objnum].OffsStatusMatrix;
 BL=16;
 loop(BL){
  IF (BL-1%5==0) ESFLOAT[ESI]=1; ELSE ESFLOAT[ESI]=0;
  ESI+=4;
 }
}


:void fastcall ClearTempMatrix()
{
 ESI=#Matrix;
 BL=16;
 loop(BL){
  IF (BL-1%5==0) DSFLOAT[ESI]=1; ELSE DSFLOAT[ESI]=0;
  ESI+=4;
 }
}

//Действия: C=A*B
//Параметры:
//Адрес матрицы A (ES)
//Адрес матрицы B (DS)
//Адрес матрицы C (ES)
:void fastcall MulMatrixAMatrixB_4x4(dword ESI, dword EDI, dword EDX)
{
  CH=4;
  loop(CH){
    CL=4;
    loop(CL){
      $fld ESFLOAT[ESI]
      $fmul DSFLOAT[EDI]
      $fld ESFLOAT[ESI+4]
      $fmul DSFLOAT[EDI+16]
      $fld ESFLOAT[ESI+8]
      $fmul DSFLOAT[EDI+32]
      $fld ESFLOAT[ESI+12]
      $fmul DSFLOAT[EDI+48]
      $faddp st(1)
      $faddp st(1)
      $faddp st(1)
      EDI+=4;
    }
    $fstp ESFLOAT[EDX+12];
    $fstp ESFLOAT[EDX+8];
    $fstp ESFLOAT[EDX+4];
    $fstp ESFLOAT[EDX];
    EDX+=16;
    ESI+=16;
    EDI-=16;
  }
}


//Действия: VectorB=A*VectorA
//Параметры:
//Адрес матрицы A
//Адрес вектора A
//Адрес вектора B
:void fastcall MulMatrixAVectorA_4x4(ESI,EDI,EDX)
{
  CH=4;
  loop(CH){
    $fld ESFLOAT[ESI]
    $fmul ESFLOAT[EDI]
    $fld ESFLOAT[ESI+4]
    $fmul ESFLOAT[EDI+4]
    $fld ESFLOAT[ESI+8]
    $fmul ESFLOAT[EDI+8]
    $fld ESFLOAT[ESI+12]
    $fmul ESFLOAT[EDI+12]
    $faddp st(1)
    $faddp st(1)
    $faddp st(1)
    ESI+=16;
  }
  $fstp ESDWORD[EDX+12];
  $fstp ESDWORD[EDX+8];
  $fstp ESDWORD[EDX+4];
  $fstp ESDWORD[EDX];
}


//Действия: C=A*B
//Параметры:
//Адрес матрицы A
//Адрес матрицы B
//Адрес матрицы C
:void fastcall MulMatrixAMatrixB_3x3(dword ESI, dword EDI, dword EDX)
{
  CH=3;
  loop(CH){
    CL=3;
    loop(CL){
      $fld ESFLOAT[ESI]
      $fmul DSFLOAT[EDI]
      $fld ESFLOAT[ESI+4]
      $fmul DSFLOAT[EDI+12]
      $fld ESFLOAT[ESI+8]
      $fmul DSFLOAT[EDI+24]
      $faddp st(1)
      $faddp st(1)
      EDI+=4;
    }
    $fstp ESFLOAT[EDX+8];
    $fstp ESFLOAT[EDX+4];
    $fstp ESFLOAT[EDX];
    EDX+=12;
    ESI+=12;
    EDI-=12;
  }
}

//Действия: VectorB=A*VectorA
//Параметры:
//Адрес матрицы A
//Адрес вектора A
//Адрес вектора B
:void fastcall MulMatrixAVectorA_3x3(ESI,EDI,EDX)
{
  CH=3;
  loop(CH){
    $fld ESFLOAT[ESI]
    $fmul ESFLOAT[EDI]
    $fld ESFLOAT[ESI+4]
    $fmul ESFLOAT[EDI+4]
    $fld ESFLOAT[ESI+8]
    $fmul ESFLOAT[EDI+8]
    $faddp st(1)
    $faddp st(1)
    ESI+=12;
  }
  $fstp ESDWORD[EDX+8];
  $fstp ESDWORD[EDX+4];
  $fstp ESDWORD[EDX];
}

//Адрес вектора A
//Адрес вектора B
//Адрес выходного вектора
:void fastcall MulVectorAVectorB(ESI,EDI,EDX)
{
 //Вычисление x вектора
 $fld DSFLOAT[ESI+4]
 $fmul DSFLOAT[EDI+8]
 $fld DSFLOAT[ESI+8]
 $fmul DSFLOAT[EDI+4]
 $fsubp ST(1)
 //Вычисление y вектора
 $fld DSFLOAT[ESI+8]
 $fmul DSFLOAT[EDI]
 $fld DSFLOAT[ESI]
 $fmul DSFLOAT[EDI+8]
 $fsubp ST(1)
 //Вычисление z вектора
 $fld DSFLOAT[ESI]
 $fmul DSFLOAT[EDI+4]
 $fld DSFLOAT[ESI+4]
 $fmul DSFLOAT[EDI]
 $fsubp ST(1)
 //Сохраняем z,y,x вектора
 $fstp DSFLOAT[EDX+8]
 $fstp DSFLOAT[EDX+4]
 $fstp DSFLOAT[EDX]
}

//Скалярное произвдение двух векторов
//Параметры:
//Адрес вектора A
//Адрес вектора B
//Адрес результата (ES)
:void fastcall ScalarMulVectorAVectorB(ESI,EDI,EDX)
{
 $fld ESFLOAT[ESI]
 $fmul ESFLOAT[EDI]
 $fld ESFLOAT[ESI+4]
 $fmul ESFLOAT[EDI+4]
 $fld ESFLOAT[ESI+8]
 $fmul ESFLOAT[EDI+8]
 $faddp ST(1)
 $faddp ST(1)
 $fstp ESFLOAT[EDX]
}

//Скалярное произвдение двух векторов
//Параметры:
//Адрес вектора A
//Адрес вектора B
//Адрес результата (DS)
:void fastcall _ScalarMulVectorAVectorB(ESI,EDI,EDX)
{
 $fld ESFLOAT[ESI]
 $fmul ESFLOAT[EDI]
 $fld ESFLOAT[ESI+4]
 $fmul ESFLOAT[EDI+4]
 $fld ESFLOAT[ESI+8]
 $fmul ESFLOAT[EDI+8]
 $faddp ST(1)
 $faddp ST(1)
 $fstp DSFLOAT[EDX]
}

//Параметры:
//ESI=offs (dword #Matrix)
:void fastcall ClearMatrix(ESI)
{
 BL=16;
 loop(BL){
  IF (BL-1%5==0) DSFLOAT[ESI]=1; ELSE DSFLOAT[ESI]=0;
  ESI+=4;
 }
}


//Действия: C=A*B
//Параметры:
//Адрес матрицы A (DS)
//Адрес матрицы B (ES)
//Адрес матрицы C (ES)
:void fastcall __MulMatrixAMatrixB_4x4(dword ESI, dword EDI, dword EDX)
 float TempMatrix[16]; //Временная матрица
{
  EBX=0;
  CH=4;
  loop(CH){
    CL=4;
    loop(CL){
      $fld DSFLOAT[ESI]
      $fmul ESFLOAT[EDI]
      $fld DSFLOAT[ESI+4]
      $fmul ESFLOAT[EDI+16]
      $fld DSFLOAT[ESI+8]
      $fmul ESFLOAT[EDI+32]
      $fld DSFLOAT[ESI+12]
      $fmul ESFLOAT[EDI+48]
      $faddp st(1)
      $faddp st(1)
      $faddp st(1)
      EDI+=4;
    }
    $fstp TempMatrix[EBX+12] //ESFLOAT[EDX+12];
    $fstp TempMatrix[EBX+8]  //ESFLOAT[EDX+8];
    $fstp TempMatrix[EBX+4]  //ESFLOAT[EDX+4];
    $fstp TempMatrix[EBX]    //ESFLOAT[EDX];
//    EDX+=16;
    EBX+=16;
    ESI+=16;
    EDI-=16;
  }
  FOR (ECX=0; ECX<64; ECX+=4) ESFLOAT[EDX+ECX]=TempMatrix[ECX];
}

//Действия: C=A*B
//Параметры:
//Адрес матрицы A (DS)
//Адрес матрицы B (DS)
//Адрес матрицы C (DS)
:void fastcall _MulMatrixAMatrixB_4x4(dword ESI, dword EDI, dword EDX)
 float TempMatrix[16]; //Временная матрица
{
  EBX=0;
  CH=4;
  loop(CH){
    CL=4;
    loop(CL){
      $fld DSFLOAT[ESI]
      $fmul DSFLOAT[EDI]
      $fld DSFLOAT[ESI+4]
      $fmul DSFLOAT[EDI+16]
      $fld DSFLOAT[ESI+8]
      $fmul DSFLOAT[EDI+32]
      $fld DSFLOAT[ESI+12]
      $fmul DSFLOAT[EDI+48]
      $faddp st(1)
      $faddp st(1)
      $faddp st(1)
      EDI+=4;
    }
    $fstp TempMatrix[EBX+12] //ESFLOAT[EDX+12];
    $fstp TempMatrix[EBX+8]  //ESFLOAT[EDX+8];
    $fstp TempMatrix[EBX+4]  //ESFLOAT[EDX+4];
    $fstp TempMatrix[EBX]    //ESFLOAT[EDX];
//    EDX+=16;
    EBX+=16;
    ESI+=16;
    EDI-=16;
  }
  FOR (ECX=0; ECX<64; ECX+=4) DSFLOAT[EDX+ECX]=TempMatrix[ECX];
}

//Действия: VectorB=A*VectorA
//Параметры:
//Адрес матрицы A (DS)
//Адрес вектора A (DS)
//Адрес вектора B (DS)
:void fastcall _MulMatrixAVectorA_4x4(ESI,EDI,EDX)
{
  CH=4;
  loop(CH){
    $fld DSFLOAT[ESI]
    $fmul DSFLOAT[EDI]
    $fld DSFLOAT[ESI+4]
    $fmul DSFLOAT[EDI+4]
    $fld DSFLOAT[ESI+8]
    $fmul DSFLOAT[EDI+8]
    $fld DSFLOAT[ESI+12]
    $fmul DSFLOAT[EDI+12]
    $faddp st(1)
    $faddp st(1)
    $faddp st(1)
    ESI+=16;
  }
  $fstp DSDWORD[EDX+12];
  $fstp DSDWORD[EDX+8];
  $fstp DSDWORD[EDX+4];
  $fstp DSDWORD[EDX];
}
