~ubuntu-branches/ubuntu/trusty/blender/trusty : revision 42

43

memset(m, 0, 4 * 4 * sizeof(float));

44

}

45

46

void unit_m3(float m[][3])

46

void unit_m3(float m[3][3])

47

{

48

m[0][0] = m[1][1] = m[2][2] = 1.0;

49

m[0][1] = m[0][2] = 0.0;

51

m[2][0] = m[2][1] = 0.0;

52

}

53

54

void unit_m4(float m[][4])

54

void unit_m4(float m[4][4])

55

{

56

m[0][0] = m[1][1] = m[2][2] = m[3][3] = 1.0;

57

m[0][1] = m[0][2] = m[0][3] = 0.0;

60

m[3][0] = m[3][1] = m[3][2] = 0.0;

61

}

62

63

void copy_m3_m3(float m1[][3], float m2[][3])

63

void copy_m3_m3(float m1[3][3], float m2[3][3])

64

{

65

/* destination comes first: */

66

memcpy(&m1[0], &m2[0], 9 * sizeof(float));

67

}

68

69

void copy_m4_m4(float m1[][4], float m2[][4])

69

void copy_m4_m4(float m1[4][4], float m2[4][4])

70

{

71

memcpy(m1, m2, 4 * 4 * sizeof(float));

72

}

73

74

void copy_m3_m4(float m1[][3], float m2[][4])

74

void copy_m3_m4(float m1[3][3], float m2[4][4])

75

{

76

m1[0][0] = m2[0][0];

77

m1[0][1] = m2[0][1];

86

m1[2][2] = m2[2][2];

87

}

88

89

void copy_m4_m3(float m1[][4], float m2[][3]) /* no clear */

89

void copy_m4_m3(float m1[4][4], float m2[3][3]) /* no clear */

90

{

91

m1[0][0] = m2[0][0];

92

m1[0][1] = m2[0][1];

112

113

}

114

115

void swap_m3m3(float m1[][3], float m2[][3])

115

void swap_m3m3(float m1[3][3], float m2[3][3])

116

{

117

float t;

118

int i, j;

126

}

127

}

128

129

void swap_m4m4(float m1[][4], float m2[][4])

129

void swap_m4m4(float m1[4][4], float m2[4][4])

130

{

131

float t;

132

int i, j;

142

143

/******************************** Arithmetic *********************************/

144

145

void mult_m4_m4m4(float m1[][4], float m3_[][4], float m2_[][4])

145

void mult_m4_m4m4(float m1[4][4], float m3_[4][4], float m2_[4][4])

146

{

147

float m2[4][4], m3[4][4];

148

173

174

}

175

176

void mul_m3_m3m3(float m1[][3], float m3_[][3], float m2_[][3])

176

void mul_m3_m3m3(float m1[3][3], float m3_[3][3], float m2_[3][3])

177

{

178

float m2[3][3], m3[3][3];

179

195

m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] + m2[2][2] * m3[2][2];

196

}

197

198

void mul_m4_m4m3(float (*m1)[4], float (*m3_)[4], float (*m2_)[3])

198

void mul_m4_m4m3(float m1[4][4], float m3_[4][4], float m2_[3][3])

199

{

200

float m2[3][3], m3[4][4];

201

215

}

216

217

/* m1 = m2 * m3, ignore the elements on the 4th row/column of m3 */

218

void mult_m3_m3m4(float m1[][3], float m3[][4], float m2[][3])

218

void mult_m3_m3m4(float m1[3][3], float m3_[4][4], float m2_[3][3])

219

{

220

float m2[3][3], m3[4][4];

221

222

/* copy so it works when m1 is the same pointer as m2 or m3 */

223

copy_m3_m3(m2, m2_);

224

copy_m4_m4(m3, m3_);

225

220

226

/* m1[i][j] = m2[i][k] * m3[k][j] */

221

227

m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] + m2[0][2] * m3[2][0];

222

228

m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] + m2[0][2] * m3[2][1];

231

237

m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] + m2[2][2] * m3[2][2];

232

238

}

233

239

234

void mul_m4_m3m4(float (*m1)[4], float (*m3)[3], float (*m2)[4])

240

void mul_m4_m3m4(float m1[4][4], float m3_[3][3], float m2_[4][4])

235

241

{

242

float m2[4][4], m3[3][3];

243

244

/* copy so it works when m1 is the same pointer as m2 or m3 */

245

copy_m4_m4(m2, m2_);

246

copy_m3_m3(m3, m3_);

247

236

248

m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] + m2[0][2] * m3[2][0];

237

249

m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] + m2[0][2] * m3[2][1];

238

250

m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] + m2[0][2] * m3[2][2];

244

256

m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] + m2[2][2] * m3[2][2];

245

257

}

246

258

247

void mul_serie_m3(float answ[][3],

248

float m1[][3], float m2[][3], float m3[][3],

249

float m4[][3], float m5[][3], float m6[][3],

250

float m7[][3], float m8[][3])

259

void mul_serie_m3(float answ[3][3],

260

float m1[3][3], float m2[3][3], float m3[3][3],

261

float m4[3][3], float m5[3][3], float m6[3][3],

262

float m7[3][3], float m8[3][3])

251

263

{

252

264

float temp[3][3];

253

265

277

289

}

278

290

}

279

291

280

void mul_serie_m4(float answ[][4], float m1[][4],

281

float m2[][4], float m3[][4], float m4[][4],

282

float m5[][4], float m6[][4], float m7[][4],

283

float m8[][4])

292

void mul_serie_m4(float answ[4][4], float m1[4][4],

293

float m2[4][4], float m3[4][4], float m4[4][4],

294

float m5[4][4], float m6[4][4], float m7[4][4],

295

float m8[4][4])

284

296

{

285

297

float temp[4][4];

286

298

310

322

}

311

323

}

312

324

313

void mul_m4_v3(float mat[][4], float vec[3])

325

void mul_m4_v3(float mat[4][4], float vec[3])

314

326

{

315

327

float x, y;

316

328

321

333

vec[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2] + mat[3][2];

322

334

}

323

335

324

void mul_v3_m4v3(float in[3], float mat[][4], const float vec[3])

336

void mul_v3_m4v3(float in[3], float mat[4][4], const float vec[3])

325

337

{

326

338

float x, y;

327

339

332

344

in[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2] + mat[3][2];

333

345

}

334

346

347

void mul_v2_m2v2(float r[2], float mat[2][2], const float vec[2])

348

{

349

float x;

350

351

x = vec[0];

352

r[0] = mat[0][0] * x + mat[1][0] * vec[1];

353

r[1] = mat[0][1] * x + mat[1][1] * vec[1];

354

}

355

335

356

/* same as mul_m4_v3() but doesnt apply translation component */

336

void mul_mat3_m4_v3(float mat[][4], float vec[3])

357

void mul_mat3_m4_v3(float mat[4][4], float vec[3])

337

358

{

338

359

float x, y;

339

360

344

365

vec[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2];

345

366

}

346

367

347

void mul_project_m4_v3(float mat[][4], float vec[3])

368

void mul_project_m4_v3(float mat[4][4], float vec[3])

348

369

{

349

370

const float w = vec[0] * mat[0][3] + vec[1] * mat[1][3] + vec[2] * mat[2][3] + mat[3][3];

350

371

mul_m4_v3(mat, vec);

354

375

vec[2] /= w;

355

376

}

356

377

357

void mul_v4_m4v4(float r[4], float mat[4][4], float v[4])

378

void mul_v4_m4v4(float r[4], float mat[4][4], const float v[4])

358

379

{

359

380

float x, y, z;

360

381

392

413

mul_v4d_m4v4d(r, mat, r);

393

414

}

394

415

395

void mul_v3_m3v3(float r[3], float M[3][3], float a[3])

416

void mul_v3_m3v3(float r[3], float M[3][3], const float a[3])

396

417

{

397

418

r[0] = M[0][0] * a[0] + M[1][0] * a[1] + M[2][0] * a[2];

398

419

r[1] = M[0][1] * a[0] + M[1][1] * a[1] + M[2][1] * a[2];

399

420

r[2] = M[0][2] * a[0] + M[1][2] * a[1] + M[2][2] * a[2];

400

421

}

401

422

423

void mul_v2_m3v3(float r[2], float M[3][3], const float a[3])

424

{

425

r[0] = M[0][0] * a[0] + M[1][0] * a[1] + M[2][0] * a[2];

426

r[1] = M[0][1] * a[0] + M[1][1] * a[1] + M[2][1] * a[2];

427

}

428

402

429

void mul_m3_v3(float M[3][3], float r[3])

403

430

{

404

431

float tmp[3];

407

434

copy_v3_v3(r, tmp);

408

435

}

409

436

410

void mul_transposed_m3_v3(float mat[][3], float vec[3])

437

void mul_transposed_m3_v3(float mat[3][3], float vec[3])

411

438

{

412

439

float x, y;

413

440

445

472

m[i][j] *= f;

446

473

}

447

474

448

void mul_m3_v3_double(float mat[][3], double vec[3])

475

void mul_m3_v3_double(float mat[3][3], double vec[3])

449

476

{

450

477

double x, y;

451

478

456

483

vec[2] = x * (double)mat[0][2] + y * (double)mat[1][2] + (double)mat[2][2] * vec[2];

457

484

}

458

485

459

void add_m3_m3m3(float m1[][3], float m2[][3], float m3[][3])

460

{

461

int i, j;

462

463

for (i = 0; i < 3; i++)

464

for (j = 0; j < 3; j++)

465

m1[i][j] = m2[i][j] + m3[i][j];

466

}

467

468

void add_m4_m4m4(float m1[][4], float m2[][4], float m3[][4])

469

{

470

int i, j;

471

472

for (i = 0; i < 4; i++)

473

for (j = 0; j < 4; j++)

474

m1[i][j] = m2[i][j] + m3[i][j];

475

}

476

477

void sub_m3_m3m3(float m1[][3], float m2[][3], float m3[][3])

478

{

479

int i, j;

480

481

for (i = 0; i < 3; i++)

482

for (j = 0; j < 3; j++)

483

m1[i][j] = m2[i][j] - m3[i][j];

484

}

485

486

void sub_m4_m4m4(float m1[][4], float m2[][4], float m3[][4])

487

{

488

int i, j;

489

490

for (i = 0; i < 4; i++)

491

for (j = 0; j < 4; j++)

492

m1[i][j] = m2[i][j] - m3[i][j];

486

void add_m3_m3m3(float m1[3][3], float m2[3][3], float m3[3][3])

487

{

488

int i, j;

489

490

for (i = 0; i < 3; i++)

491

for (j = 0; j < 3; j++)

492

m1[i][j] = m2[i][j] + m3[i][j];

493

}

494

495

void add_m4_m4m4(float m1[4][4], float m2[4][4], float m3[4][4])

496

{

497

int i, j;

498

499

for (i = 0; i < 4; i++)

500

for (j = 0; j < 4; j++)

501

m1[i][j] = m2[i][j] + m3[i][j];

502

}

503

504

void sub_m3_m3m3(float m1[3][3], float m2[3][3], float m3[3][3])

505

{

506

int i, j;

507

508

for (i = 0; i < 3; i++)

509

for (j = 0; j < 3; j++)

510

m1[i][j] = m2[i][j] - m3[i][j];

511

}

512

513

void sub_m4_m4m4(float m1[4][4], float m2[4][4], float m3[4][4])

514

{

515

int i, j;

516

517

for (i = 0; i < 4; i++)

518

for (j = 0; j < 4; j++)

519

m1[i][j] = m2[i][j] - m3[i][j];

520

}

521

522

float determinant_m3_array(float m[3][3])

523

{

524

return (m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1]) -

525

m[1][0] * (m[0][1] * m[2][2] - m[0][2] * m[2][1]) +

526

m[2][0] * (m[0][1] * m[1][2] - m[0][2] * m[1][1]));

527

}

528

529

int invert_m3_ex(float m[3][3], const float epsilon)

530

{

531

float tmp[3][3];

532

int success;

533

534

success = invert_m3_m3_ex(tmp, m, epsilon);

535

copy_m3_m3(m, tmp);

536

537

return success;

538

}

539

540

int invert_m3_m3_ex(float m1[3][3], float m2[3][3], const float epsilon)

541

{

542

float det;

543

int a, b, success;

544

545

BLI_assert(epsilon >= 0.0f);

546

547

/* calc adjoint */

548

adjoint_m3_m3(m1, m2);

549

550

/* then determinant old matrix! */

551

det = determinant_m3_array(m2);

552

553

success = (fabsf(det) > epsilon);

554

555

if (LIKELY(det != 0.0f)) {

556

det = 1.0f / det;

557

for (a = 0; a < 3; a++) {

558

for (b = 0; b < 3; b++) {

559

m1[a][b] *= det;

560

}

561

}

562

}

563

return success;

493

564

}

494

565

495

566

int invert_m3(float m[3][3])

512

583

adjoint_m3_m3(m1, m2);

513

584

514

585

/* then determinant old matrix! */

515

det = (m2[0][0] * (m2[1][1] * m2[2][2] - m2[1][2] * m2[2][1]) -

516

m2[1][0] * (m2[0][1] * m2[2][2] - m2[0][2] * m2[2][1]) +

517

m2[2][0] * (m2[0][1] * m2[1][2] - m2[0][2] * m2[1][1]));

518

519

success = (det != 0);

520

521

if (det == 0) det = 1;

522

det = 1 / det;

523

for (a = 0; a < 3; a++) {

524

for (b = 0; b < 3; b++) {

525

m1[a][b] *= det;

586

det = determinant_m3_array(m2);

587

588

success = (det != 0.0f);

589

590

if (LIKELY(det != 0.0f)) {

591

det = 1.0f / det;

592

for (a = 0; a < 3; a++) {

593

for (b = 0; b < 3; b++) {

594

m1[a][b] *= det;

595

}

526

596

}

527

597

}

528

598

557

627

float max;

558

628

int maxj;

559

629

630

BLI_assert(inverse != mat);

631

560

632

/* Set inverse to identity */

561

633

for (i = 0; i < 4; i++)

562

634

for (j = 0; j < 4; j++)

591

663

if (temp == 0)

592

664

return 0; /* No non-zero pivot */

593

665

for (k = 0; k < 4; k++) {

594

tempmat[i][k] = (float)(tempmat[i][k] / temp);

595

inverse[i][k] = (float)(inverse[i][k] / temp);

666

tempmat[i][k] = (float)((double)tempmat[i][k] / temp);

667

inverse[i][k] = (float)((double)inverse[i][k] / temp);

596

668

}

597

669

for (j = 0; j < 4; j++) {

598

670

if (j != i) {

599

671

temp = tempmat[j][i];

600

672

for (k = 0; k < 4; k++) {

601

tempmat[j][k] -= (float)(tempmat[i][k] * temp);

602

inverse[j][k] -= (float)(inverse[i][k] * temp);

673

tempmat[j][k] -= (float)((double)tempmat[i][k] * temp);

674

inverse[j][k] -= (float)((double)inverse[i][k] * temp);

603

675

}

604

676

}

605

677

}

609

681

610

682

/****************************** Linear Algebra *******************************/

611

683

612

void transpose_m3(float mat[][3])

684

void transpose_m3(float mat[3][3])

613

685

{

614

686

float t;

615

687

624

696

mat[2][1] = t;

625

697

}

626

698

627

void transpose_m4(float mat[][4])

699

void transpose_m4(float mat[4][4])

628

700

{

629

701

float t;

630

702

650

722

mat[3][2] = t;

651

723

}

652

724

653

void orthogonalize_m3(float mat[][3], int axis)

725

void orthogonalize_m3(float mat[3][3], int axis)

654

726

{

655

727

float size[3];

656

728

mat3_to_size(size, mat);

728

800

mul_v3_fl(mat[2], size[2]);

729

801

}

730

802

731

void orthogonalize_m4(float mat[][4], int axis)

803

void orthogonalize_m4(float mat[4][4], int axis)

732

804

{

733

805

float size[3];

734

806

mat4_to_size(size, mat);

807

879

mul_v3_fl(mat[2], size[2]);

808

880

}

809

881

810

int is_orthogonal_m3(float m[][3])

882

int is_orthogonal_m3(float m[3][3])

811

883

{

812

884

int i, j;

813

885

821

893

return 1;

822

894

}

823

895

824

int is_orthogonal_m4(float m[][4])

896

int is_orthogonal_m4(float m[4][4])

825

897

{

826

898

int i, j;

827

899

836

908

return 1;

837

909

}

838

910

839

int is_orthonormal_m3(float m[][3])

911

int is_orthonormal_m3(float m[3][3])

840

912

{

841

913

if (is_orthogonal_m3(m)) {

842

914

int i;

851

923

return 0;

852

924

}

853

925

854

int is_orthonormal_m4(float m[][4])

926

int is_orthonormal_m4(float m[4][4])

855

927

{

856

928

if (is_orthogonal_m4(m)) {

857

929

int i;

866

938

return 0;

867

939

}

868

940

869

int is_uniform_scaled_m3(float m[][3])

941

int is_uniform_scaled_m3(float m[3][3])

870

942

{

871

943

const float eps = 1e-7;

872

944

float t[3][3];

895

967

return 0;

896

968

}

897

969

898

void normalize_m3(float mat[][3])

970

void normalize_m3(float mat[3][3])

899

971

{

900

972

normalize_v3(mat[0]);

901

973

normalize_v3(mat[1]);

902

974

normalize_v3(mat[2]);

903

975

}

904

976

905

void normalize_m3_m3(float rmat[][3], float mat[][3])

977

void normalize_m3_m3(float rmat[3][3], float mat[3][3])

906

978

{

907

979

normalize_v3_v3(rmat[0], mat[0]);

908

980

normalize_v3_v3(rmat[1], mat[1]);

909

981

normalize_v3_v3(rmat[2], mat[2]);

910

982

}

911

983

912

void normalize_m4(float mat[][4])

984

void normalize_m4(float mat[4][4])

913

985

{

914

986

float len;

915

987

921

993

if (len != 0.0f) mat[2][3] /= len;

922

994

}

923

995

924

void normalize_m4_m4(float rmat[][4], float mat[][4])

925

{

926

float len;

927

928

len = normalize_v3_v3(rmat[0], mat[0]);

929

if (len != 0.0f) rmat[0][3] = mat[0][3] / len;

930

len = normalize_v3_v3(rmat[1], mat[1]);

931

if (len != 0.0f) rmat[1][3] = mat[1][3] / len;

932

len = normalize_v3_v3(rmat[2], mat[2]);

933

if (len != 0.0f) rmat[2][3] = mat[2][3] / len;

934

}

935

936

void adjoint_m3_m3(float m1[][3], float m[][3])

937

{

996

void normalize_m4_m4(float rmat[4][4], float mat[4][4])

997

{

998

copy_m4_m4(rmat, mat);

999

normalize_m4(rmat);

1000

}

1001

1002

void adjoint_m2_m2(float m1[2][2], float m[2][2])

1003

{

1004

BLI_assert(m1 != m);

1005

m1[0][0] = m[1][1];

1006

m1[0][1] = -m[1][0];

1007

m1[1][0] = -m[0][1];

1008

m1[1][1] = m[0][0];

1009

}

1010

1011

void adjoint_m3_m3(float m1[3][3], float m[3][3])

1012

{

1013

BLI_assert(m1 != m);

938

1014

m1[0][0] = m[1][1] * m[2][2] - m[1][2] * m[2][1];

939

1015

m1[0][1] = -m[0][1] * m[2][2] + m[0][2] * m[2][1];

940

1016

m1[0][2] = m[0][1] * m[1][2] - m[0][2] * m[1][1];

948

1024

m1[2][2] = m[0][0] * m[1][1] - m[0][1] * m[1][0];

949

1025

}

950

1026

951

void adjoint_m4_m4(float out[][4], float in[][4]) /* out = ADJ(in) */

1027

void adjoint_m4_m4(float out[4][4], float in[4][4]) /* out = ADJ(in) */

952

1028

{

953

1029

float a1, a2, a3, a4, b1, b2, b3, b4;

954

1030

float c1, c2, c3, c4, d1, d2, d3, d4;

1014

1090

return ans;

1015

1091

}

1016

1092

1017

float determinant_m4(float m[][4])

1093

float determinant_m4(float m[4][4])

1018

1094

{

1019

1095

float ans;

1020

1096

float a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4;

1049

1125

1050

1126

/****************************** Transformations ******************************/

1051

1127

1052

void size_to_mat3(float mat[][3], const float size[3])

1128

void size_to_mat3(float mat[3][3], const float size[3])

1053

1129

{

1054

1130

mat[0][0] = size[0];

1055

1131

mat[0][1] = 0.0f;

1062

1138

mat[2][0] = 0.0f;

1063

1139

}

1064

1140

1065

void size_to_mat4(float mat[][4], const float size[3])

1141

void size_to_mat4(float mat[4][4], const float size[3])

1066

1142

{

1067

1143

float tmat[3][3];

1068

1144

1071

1147

copy_m4_m3(mat, tmat);

1072

1148

}

1073

1149

1074

void mat3_to_size(float size[3], float mat[][3])

1150

void mat3_to_size(float size[3], float mat[3][3])

1075

1151

{

1076

1152

size[0] = len_v3(mat[0]);

1077

1153

size[1] = len_v3(mat[1]);

1078

1154

size[2] = len_v3(mat[2]);

1079

1155

}

1080

1156

1081

void mat4_to_size(float size[3], float mat[][4])

1157

void mat4_to_size(float size[3], float mat[4][4])

1082

1158

{

1083

1159

size[0] = len_v3(mat[0]);

1084

1160

size[1] = len_v3(mat[1]);

1088

1164

/* this gets the average scale of a matrix, only use when your scaling

1089

1165

* data that has no idea of scale axis, examples are bone-envelope-radius

1090

1166

* and curve radius */

1091

float mat3_to_scale(float mat[][3])

1167

float mat3_to_scale(float mat[3][3])

1092

1168

{

1093

1169

/* unit length vector */

1094

1170

float unit_vec[3] = {0.577350269189626f, 0.577350269189626f, 0.577350269189626f};

1096

1172

return len_v3(unit_vec);

1097

1173

}

1098

1174

1099

float mat4_to_scale(float mat[][4])

1175

float mat4_to_scale(float mat[4][4])

1100

1176

{

1101

1177

float tmat[3][3];

1102

1178

copy_m3_m4(tmat, mat);

1111

1187

/* rotation & scale are linked, we need to create the mat's

1112

1188

* for these together since they are related. */

1113

1189

1114

/* so scale doesnt interfear with rotation [#24291] */

1190

/* so scale doesn't interfere with rotation [#24291] */

1115

1191

/* note: this is a workaround for negative matrix not working for rotation conversion, FIXME */

1116

1192

normalize_m3_m3(mat3_n, mat3);

1117

1193

if (is_negative_m3(mat3)) {

1134

1210

size[2] = mat3[2][2];

1135

1211

}

1136

1212

1137

void mat4_to_loc_rot_size(float loc[3], float rot[3][3], float size[3], float wmat[][4])

1213

void mat4_to_loc_rot_size(float loc[3], float rot[3][3], float size[3], float wmat[4][4])

1138

1214

{

1139

1215

float mat3[3][3]; /* wmat -> 3x3 */

1140

1216

1145

1221

copy_v3_v3(loc, wmat[3]);

1146

1222

}

1147

1223

1148

void scale_m3_fl(float m[][3], float scale)

1224

void mat4_to_loc_quat(float loc[3], float quat[4], float wmat[4][4])

1225

{

1226

float mat3[3][3];

1227

float mat3_n[3][3]; /* normalized mat3 */

1228

1229

copy_m3_m4(mat3, wmat);

1230

normalize_m3_m3(mat3_n, mat3);

1231

1232

/* so scale doesn't interfere with rotation [#24291] */

1233

/* note: this is a workaround for negative matrix not working for rotation conversion, FIXME */

1234

if (is_negative_m3(mat3)) {

1235

negate_v3(mat3_n[0]);

1236

negate_v3(mat3_n[1]);

1237

negate_v3(mat3_n[2]);

1238

}

1239

1240

mat3_to_quat(quat, mat3_n);

1241

copy_v3_v3(loc, wmat[3]);

1242

}

1243

1244

void mat4_decompose(float loc[3], float quat[4], float size[3], float wmat[4][4])

1245

{

1246

float rot[3][3];

1247

mat4_to_loc_rot_size(loc, rot, size, wmat);

1248

mat3_to_quat(quat, rot);

1249

}

1250

1251

void scale_m3_fl(float m[3][3], float scale)

1149

1252

{

1150

1253

m[0][0] = m[1][1] = m[2][2] = scale;

1151

1254

m[0][1] = m[0][2] = 0.0;

1153

1256

m[2][0] = m[2][1] = 0.0;

1154

1257

}

1155

1258

1156

void scale_m4_fl(float m[][4], float scale)

1259

void scale_m4_fl(float m[4][4], float scale)

1157

1260

{

1158

1261

m[0][0] = m[1][1] = m[2][2] = scale;

1159

1262

m[3][3] = 1.0;

1163

1266

m[3][0] = m[3][1] = m[3][2] = 0.0;

1164

1267

}

1165

1268

1166

void translate_m4(float mat[][4], float Tx, float Ty, float Tz)

1269

void translate_m4(float mat[4][4], float Tx, float Ty, float Tz)

1167

1270

{

1168

1271

mat[3][0] += (Tx * mat[0][0] + Ty * mat[1][0] + Tz * mat[2][0]);

1169

1272

mat[3][1] += (Tx * mat[0][1] + Ty * mat[1][1] + Tz * mat[2][1]);

1170

1273

mat[3][2] += (Tx * mat[0][2] + Ty * mat[1][2] + Tz * mat[2][2]);

1171

1274

}

1172

1275

1173

void rotate_m4(float mat[][4], const char axis, const float angle)

1276

void rotate_m4(float mat[4][4], const char axis, const float angle)

1174

1277

{

1175

1278

int col;

1176

1279

float temp[4] = {0.0f, 0.0f, 0.0f, 0.0f};

1178

1281

1179

1282

assert(axis >= 'X' && axis <= 'Z');

1180

1283

1181

cosine = (float)cos(angle);

1182

sine = (float)sin(angle);

1284

cosine = cosf(angle);

1285

sine = sinf(angle);

1183

1286

switch (axis) {

1184

1287

case 'X':

1185

1288

for (col = 0; col < 4; col++)

1210

1313

}

1211

1314

}

1212

1315

1213

void blend_m3_m3m3(float out[][3], float dst[][3], float src[][3], const float srcweight)

1316

void rotate_m2(float mat[2][2], const float angle)

1317

{

1318

mat[0][0] = mat[1][1] = cosf(angle);

1319

mat[0][1] = sinf(angle);

1320

mat[1][0] = -mat[0][1];

1321

}

1322

1323

void blend_m3_m3m3(float out[3][3], float dst[3][3], float src[3][3], const float srcweight)

1214

1324

{

1215

1325

float srot[3][3], drot[3][3];

1216

1326

float squat[4], dquat[4], fquat[4];

1233

1343

mul_m3_m3m3(out, rmat, smat);

1234

1344

}

1235

1345

1236

void blend_m4_m4m4(float out[][4], float dst[][4], float src[][4], const float srcweight)

1346

void blend_m4_m4m4(float out[4][4], float dst[4][4], float src[4][4], const float srcweight)

1237

1347

{

1238

1348

float sloc[3], dloc[3], floc[3];

1239

1349

float srot[3][3], drot[3][3];

1255

1365

loc_quat_size_to_mat4(out, floc, fquat, fsize);

1256

1366

}

1257

1367

1258

int is_negative_m3(float mat[][3])

1368

int is_negative_m3(float mat[3][3])

1259

1369

{

1260

1370

float vec[3];

1261

1371

cross_v3_v3v3(vec, mat[0], mat[1]);

1262

1372

return (dot_v3v3(vec, mat[2]) < 0.0f);

1263

1373

}

1264

1374

1265

int is_negative_m4(float mat[][4])

1375

int is_negative_m4(float mat[4][4])

1266

1376

{

1267

1377

float vec[3];

1268

1378

cross_v3_v3v3(vec, mat[0], mat[1]);

1271

1381

1272

1382

/* make a 4x4 matrix out of 3 transform components */

1273

1383

/* matrices are made in the order: scale * rot * loc */

1274

// TODO: need to have a version that allows for rotation order...

1384

/* TODO: need to have a version that allows for rotation order... */

1275

1385

1276

1386

void loc_eul_size_to_mat4(float mat[4][4], const float loc[3], const float eul[3], const float size[3])

1277

1387

{

1352

1462

1353

1463

/*********************************** Other ***********************************/

1354

1464

1355

void print_m3(const char *str, float m[][3])

1465

void print_m3(const char *str, float m[3][3])

1356

1466

{

1357

1467

printf("%s\n", str);

1358

1468

printf("%f %f %f\n", m[0][0], m[1][0], m[2][0]);

1361

1471

printf("\n");

1362

1472

}

1363

1473

1364

void print_m4(const char *str, float m[][4])

1474

void print_m4(const char *str, float m[4][4])

1365

1475

{

1366

1476

printf("%s\n", str);

1367

1477

printf("%f %f %f %f\n", m[0][0], m[1][0], m[2][0], m[3][0]);

1388

1498

int m = 4;

1389

1499

int n = 4;

1390

1500

int maxiter = 200;

1391

int nu = minf(m, n);

1501

int nu = min_ff(m, n);

1392

1502

1393

1503

float *work = work1;

1394

1504

float *e = work2;

1396

1506

1397

1507

int i = 0, j = 0, k = 0, p, pp, iter;

1398

1508

1399

// Reduce A to bidiagonal form, storing the diagonal elements

1400

// in s and the super-diagonal elements in e.

1509

/* Reduce A to bidiagonal form, storing the diagonal elements

1510

* in s and the super-diagonal elements in e. */

1401

1511

1402

int nct = minf(m - 1, n);

1403

int nrt = maxf(0, minf(n - 2, m));

1512

int nct = min_ff(m - 1, n);

1513

int nrt = max_ff(0, min_ff(n - 2, m));

1404

1514

1405

1515

copy_m4_m4(A, A_);

1406

1516

zero_m4(U);

1407

1517

zero_v4(s);

1408

1518

1409

for (k = 0; k < maxf(nct, nrt); k++) {

1519

for (k = 0; k < max_ff(nct, nrt); k++) {

1410

1520

if (k < nct) {

1411

1521

1412

// Compute the transformation for the k-th column and

1413

// place the k-th diagonal in s[k].

1414

// Compute 2-norm of k-th column without under/overflow.

1522

/* Compute the transformation for the k-th column and

1523

* place the k-th diagonal in s[k].

1524

* Compute 2-norm of k-th column without under/overflow. */

1415

1525

s[k] = 0;

1416

1526

for (i = k; i < m; i++) {

1417

1527

s[k] = hypotf(s[k], A[i][k]);

1432

1542

for (j = k + 1; j < n; j++) {

1433

1543

if ((k < nct) && (s[k] != 0.0f)) {

1434

1544

1435

// Apply the transformation.

1545

/* Apply the transformation. */

1436

1546

1437

1547

float t = 0;

1438

1548

for (i = k; i < m; i++) {

1444

1554

}

1445

1555

}

1446

1556

1447

// Place the k-th row of A into e for the

1448

// subsequent calculation of the row transformation.

1557

/* Place the k-th row of A into e for the */

1558

/* subsequent calculation of the row transformation. */

1449

1559

1450

1560

e[j] = A[k][j];

1451

1561

}

1452

1562

if (k < nct) {

1453

1563

1454

// Place the transformation in U for subsequent back

1455

// multiplication.

1564

/* Place the transformation in U for subsequent back

1565

* multiplication. */

1456

1566

1457

1567

for (i = k; i < m; i++)

1458

1568

U[i][k] = A[i][k];

1459

1569

}

1460

1570

if (k < nrt) {

1461

1571

1462

// Compute the k-th row transformation and place the

1463

// k-th super-diagonal in e[k].

1464

// Compute 2-norm without under/overflow.

1572

/* Compute the k-th row transformation and place the

1573

* k-th super-diagonal in e[k].

1574

* Compute 2-norm without under/overflow. */

1465

1575

e[k] = 0;

1466

1576

for (i = k + 1; i < n; i++) {

1467

1577

e[k] = hypotf(e[k], e[i]);

1481

1591

if ((k + 1 < m) & (e[k] != 0.0f)) {

1482

1592

float invek1;

1483

1593

1484

// Apply the transformation.

1594

/* Apply the transformation. */

1485

1595

1486

1596

for (i = k + 1; i < m; i++) {

1487

1597

work[i] = 0.0f;

1500

1610

}

1501

1611

}

1502

1612

1503

// Place the transformation in V for subsequent

1504

// back multiplication.

1613

/* Place the transformation in V for subsequent

1614

* back multiplication. */

1505

1615

1506

1616

for (i = k + 1; i < n; i++)

1507

1617

V[i][k] = e[i];

1508

1618

}

1509

1619

}

1510

1620

1511

// Set up the final bidiagonal matrix or order p.

1621

/* Set up the final bidiagonal matrix or order p. */

1512

1622

1513

p = minf(n, m + 1);

1623

p = min_ff(n, m + 1);

1514

1624

if (nct < n) {

1515

1625

s[nct] = A[nct][nct];

1516

1626

}

1522

1632

}

1523

1633

e[p - 1] = 0.0f;

1524

1634

1525

// If required, generate U.

1635

/* If required, generate U. */

1526

1636

1527

1637

for (j = nct; j < nu; j++) {

1528

1638

for (i = 0; i < m; i++) {

1558

1668

}

1559

1669

}

1560

1670

1561

// If required, generate V.

1671

/* If required, generate V. */

1562

1672

1563

1673

for (k = n - 1; k >= 0; k--) {

1564

1674

if ((k < nrt) & (e[k] != 0.0f)) {

1579

1689

V[k][k] = 1.0f;

1580

1690

}

1581

1691

1582

// Main iteration loop for the singular values.

1692

/* Main iteration loop for the singular values. */

1583

1693

1584

1694

pp = p - 1;

1585

1695

iter = 0;

1587

1697

while (p > 0) {

1588

1698

int kase = 0;

1589

1699

1590

// Test for maximum iterations to avoid infinite loop

1700

/* Test for maximum iterations to avoid infinite loop */

1591

1701

if (maxiter == 0)

1592

1702

break;

1593

1703

maxiter--;

1594

1704

1595

// This section of the program inspects for

1596

// negligible elements in the s and e arrays. On

1597

// completion the variables kase and k are set as follows.

1598

1599

// kase = 1 if s(p) and e[k - 1] are negligible and k<p

1600

// kase = 2 if s(k) is negligible and k<p

1601

// kase = 3 if e[k - 1] is negligible, k<p, and

1602

// s(k), ..., s(p) are not negligible (qr step).

1603

// kase = 4 if e(p - 1) is negligible (convergence).

1705

/* This section of the program inspects for

1706

* negligible elements in the s and e arrays. On

1707

* completion the variables kase and k are set as follows.

1708

*

1709

* kase = 1 if s(p) and e[k - 1] are negligible and k<p

1710

* kase = 2 if s(k) is negligible and k<p

1711

* kase = 3 if e[k - 1] is negligible, k<p, and

1712

* s(k), ..., s(p) are not negligible (qr step).

1713

* kase = 4 if e(p - 1) is negligible (convergence). */

1604

1714

1605

1715

for (k = p - 2; k >= -1; k--) {

1606

1716

if (k == -1) {

1641

1751

}

1642

1752

k++;

1643

1753

1644

// Perform the task indicated by kase.

1754

/* Perform the task indicated by kase. */

1645

1755

1646

1756

switch (kase) {

1647

1757

1648

// Deflate negligible s(p).

1758

/* Deflate negligible s(p). */

1649

1759

1650

1760

case 1:

1651

1761

{

1671

1781

break;

1672

1782

}

1673

1783

1674

// Split at negligible s(k).

1784

/* Split at negligible s(k). */

1675

1785

1676

1786

case 2:

1677

1787

{

1695

1805

break;

1696

1806

}

1697

1807

1698

// Perform one qr step.

1808

/* Perform one qr step. */

1699

1809

1700

1810

case 3:

1701

1811

{

1702

1812

1703

// Calculate the shift.

1813

/* Calculate the shift. */

1704

1814

1705

float scale = maxf(maxf(maxf(maxf(

1706

fabsf(s[p - 1]),fabsf(s[p - 2])),fabsf(e[p - 2])),

1707

fabsf(s[k])),fabsf(e[k]));

1815

float scale = max_ff(max_ff(max_ff(max_ff(

1816

fabsf(s[p - 1]), fabsf(s[p - 2])), fabsf(e[p - 2])),

1817

fabsf(s[k])), fabsf(e[k]));

1708

1818

float invscale = 1.0f / scale;

1709

1819

float sp = s[p - 1] * invscale;

1710

1820

float spm1 = s[p - 2] * invscale;

1725

1835

f = (sk + sp) * (sk - sp) + shift;

1726

1836

g = sk * ek;

1727

1837

1728

// Chase zeros.

1838

/* Chase zeros. */

1729

1839

1730

1840

for (j = k; j < p - 1; j++) {

1731

1841

float t = hypotf(f, g);

1767

1877

iter = iter + 1;

1768

1878

break;

1769

1879

}

1770

// Convergence.

1880

/* Convergence. */

1771

1881

1772

1882

case 4:

1773

1883

{

1774

1884

1775

// Make the singular values positive.

1885

/* Make the singular values positive. */

1776

1886

1777

1887

if (s[k] <= 0.0f) {

1778

1888

s[k] = (s[k] < 0.0f ? -s[k] : 0.0f);

1781

1891

V[i][k] = -V[i][k];

1782

1892

}

1783

1893

1784

// Order the singular values.

1894

/* Order the singular values. */

1785

1895

1786

1896

while (k < pp) {

1787

1897

float t;

1835

1945

1836

1946

mul_serie_m4(Ainv, U, Wm, V, NULL, NULL, NULL, NULL, NULL);

1837

1947

}

1948

1949

void pseudoinverse_m3_m3(float Ainv[3][3], float A[3][3], float epsilon)

1950

{

1951

/* try regular inverse when possible, otherwise fall back to slow svd */

1952

if (!invert_m3_m3(Ainv, A)) {

1953

float tmp[4][4], tmpinv[4][4];

1954

1955

copy_m4_m3(tmp, A);

1956

pseudoinverse_m4_m4(tmpinv, tmp, epsilon);

1957

copy_m3_m4(Ainv, tmpinv);

1958

}

1959

}

1960