world.cc 30.3 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20
/* This file is part of StepCore library.
   Copyright (C) 2007 Vladimir Kuznetsov <ks.vladimir@gmail.com>

   StepCore library is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 2 of the License, or
   (at your option) any later version.

   StepCore library is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with StepCore; if not, write to the Free Software
   Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
*/

#include "world.h"
#include "solver.h"
21
#include "collisionsolver.h"
22
#include "constraintsolver.h"
23
#include <algorithm>
24
#include <cmath>
25
#include <QtGlobal>
26 27 28 29

namespace StepCore
{

30
STEPCORE_META_OBJECT(Item, QT_TRANSLATE_NOOP("ObjectClass", "Item"), QT_TR_NOOP("Item"), MetaObject::ABSTRACT, STEPCORE_SUPER_CLASS(Object),
Vladimir Kuznetsov's avatar
Vladimir Kuznetsov committed
31
        STEPCORE_PROPERTY_RW(StepCore::Color, color, QT_TRANSLATE_NOOP("PropertyName", "color"), STEPCORE_UNITS_NULL, QT_TR_NOOP("Item color"), color, setColor))
32 33 34
STEPCORE_META_OBJECT(Force, QT_TRANSLATE_NOOP("ObjectClass", "Force"), QT_TR_NOOP("Force"), MetaObject::ABSTRACT,,)
STEPCORE_META_OBJECT(Joint, QT_TRANSLATE_NOOP("ObjectClass", "Joint"), QT_TR_NOOP("Joint"), MetaObject::ABSTRACT,,)
STEPCORE_META_OBJECT(Tool, QT_TRANSLATE_NOOP("ObjectClass", "Tool"), QT_TR_NOOP("Tool"), MetaObject::ABSTRACT,,)
35

36
STEPCORE_META_OBJECT(ObjectErrors, QT_TRANSLATE_NOOP("ObjectClass", "ObjectErrors"), QT_TR_NOOP("ObjectErrors"), MetaObject::ABSTRACT, STEPCORE_SUPER_CLASS(Object),)
37

38
STEPCORE_META_OBJECT(ItemGroup, QT_TRANSLATE_NOOP("ObjectClass", "ItemGroup"), QT_TR_NOOP("ItemGroup"), 0, STEPCORE_SUPER_CLASS(Item),)
39

40
STEPCORE_META_OBJECT(World, QT_TRANSLATE_NOOP("ObjectClass", "World"), QT_TR_NOOP("World"), 0, STEPCORE_SUPER_CLASS(ItemGroup),
Vladimir Kuznetsov's avatar
Vladimir Kuznetsov committed
41
        STEPCORE_PROPERTY_RW_D(double, time, QT_TRANSLATE_NOOP("PropertyName", "time"), QT_TRANSLATE_NOOP("Units", "s"), QT_TR_NOOP("Current time"), time, setTime)
42 43
        STEPCORE_PROPERTY_RW  (double, timeScale, QT_TR_NOOP("timeScale"), STEPCORE_UNITS_1, QT_TR_NOOP("Simulation speed scale"), timeScale, setTimeScale)
        STEPCORE_PROPERTY_RW  (bool, errorsCalculation, QT_TR_NOOP("errorsCalculation"), STEPCORE_UNITS_NULL,
Andrew Coles's avatar
Andrew Coles committed
44
                        QT_TR_NOOP("Enable global error calculation"), errorsCalculation, setErrorsCalculation))
45 46 47 48 49 50 51 52

Item& Item::operator=(const Item& item)
{
    Object::operator=(item);

    _world = item._world;
    _group = item._group;

53 54 55 56
    if(item._objectErrors) {
        _objectErrors = static_cast<ObjectErrors*>(
            item._objectErrors->metaObject()->cloneObject(*item._objectErrors) );
        _objectErrors->setOwner(this);
57
    } else {
58
        _objectErrors = NULL;
59 60
    }

61 62
    _color = item._color;

63 64 65
    return *this;
}

66
ObjectErrors* Item::objectErrors()
67
{
68 69
    if(!_objectErrors) _objectErrors = createObjectErrors();
    return _objectErrors;
70
}
71

72
ItemGroup::ItemGroup(const ItemGroup& group)
73
    : Item()
74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191
{
    *this = group;
}

void ItemGroup::setWorld(World* world)
{
    ItemList::const_iterator end = _items.end();
    for(ItemList::const_iterator it = _items.begin(); it != end; ++it)
        (*it)->setWorld(world);
    Item::setWorld(world);
}

void ItemGroup::worldItemRemoved(Item* item)
{
    ItemList::const_iterator end = _items.end();
    for(ItemList::const_iterator it = _items.begin(); it != end; ++it)
        (*it)->worldItemRemoved(item);
}

void ItemGroup::addItem(Item* item)
{
    _items.push_back(item);

    if(world()) world()->worldItemAdded(item);

    item->setGroup(this);
    item->setWorld(this->world());
}

void ItemGroup::removeItem(Item* item)
{
    item->setWorld(NULL);
    item->setGroup(NULL);

    if(world()) world()->worldItemRemoved(item);

    ItemList::iterator i = std::find(_items.begin(), _items.end(), item);
    STEPCORE_ASSERT_NOABORT(i != _items.end());
    _items.erase(i);
}

void ItemGroup::clear()
{
    ItemList::const_iterator end = _items.end();
    for(ItemList::const_iterator it = _items.begin(); it != end; ++it) {
        (*it)->setWorld(NULL);
        (*it)->setGroup(NULL);
        if(world()) world()->worldItemRemoved(*it);
    }

    for(ItemList::const_iterator it = _items.begin(); it != end; ++it) {
        delete *it;
    }

    _items.clear();
}

ItemGroup::~ItemGroup()
{
    clear();
}

ItemGroup& ItemGroup::operator=(const ItemGroup& group)
{

    /*
    item->setGroup(this);
    item->setWorld(this->world());
    _items.push_back(item);

    if(world()) {
        //world()->worldItemAdded(item);
        ItemGroup* gr = dynamic_cast<ItemGroup*>(item);
        if(gr) gr->groupItemsAdded();
    }
    */

    if(this == &group) return *this;

    clear();

    _items.reserve(group._items.size());

    const ItemList::const_iterator gr_end = group._items.end();
    for(ItemList::const_iterator it = group._items.begin(); it != gr_end; ++it) {
        StepCore::Item* item = static_cast<Item*>( (*it)->metaObject()->cloneObject(*(*it)) );
        _items.push_back(item);
    }

    const ItemList::const_iterator end = _items.end();
    for(ItemList::const_iterator it = _items.begin(); it != end; ++it) {
        (*it)->setGroup(this);
    }

    Item::operator=(group);

    // NOTE: We don't change world() here

    return *this;
}

int ItemGroup::childItemIndex(const Item* item) const
{
    ItemList::const_iterator o = std::find(_items.begin(), _items.end(), item);
    STEPCORE_ASSERT_NOABORT(o != _items.end());
    return std::distance(_items.begin(), o);
}

Item* ItemGroup::childItem(const QString& name) const
{
    ItemList::const_iterator end = _items.end();
    for(ItemList::const_iterator it = _items.begin(); it != end; ++it)
        if((*it)->name() == name) return *it;
    return NULL;
}

Item* ItemGroup::item(const QString& name) const
{
192
    if(name.isEmpty()) return NULL;
193 194 195
    ItemList::const_iterator end = _items.end();
    for(ItemList::const_iterator it = _items.begin(); it != end; ++it) {
        if((*it)->name() == name) return *it;
196 197 198
        if((*it)->metaObject()->inherits<ItemGroup>()) {
            Item* ret = static_cast<ItemGroup*>(*it)->item(name);
            if(ret) return ret;
199 200 201 202 203
        }
    }
    return NULL;
}

204 205 206 207 208 209 210 211 212 213 214
void ItemGroup::allItems(ItemList* items) const
{
    items->reserve(_items.size());
    ItemList::const_iterator end = _items.end();
    for(ItemList::const_iterator it = _items.begin(); it != end; ++it) {
        items->push_back(*it);
        if((*it)->metaObject()->inherits<ItemGroup>())
            static_cast<ItemGroup*>(*it)->allItems(items);
    }
}

215
void ConstraintsInfo::setDimension(int newVariablesCount, int newConstraintsCount, int newContactsCount)
216
{
217 218 219 220 221 222 223
//     std::cerr << "   ConstraintsInfo::setDimension("
//       << newVariablesCount <<","<< newConstraintsCount <<"," << newContactsCount << ")\n";
    
    int totalConstraintsCount = newConstraintsCount+newContactsCount;

    jacobian.resize(totalConstraintsCount, newVariablesCount);
    jacobianDerivative.resize(totalConstraintsCount, newVariablesCount);
224
    inverseMass.resize(newVariablesCount);
225 226 227 228 229 230 231 232 233 234 235 236 237 238
    force.resize(newVariablesCount);
    value.resize(totalConstraintsCount);
    derivative.resize(totalConstraintsCount);
    if (totalConstraintsCount>0)
    {
      derivative.setZero();
      value.setZero();
    }
    forceMin.resize(totalConstraintsCount);
    forceMax.resize(totalConstraintsCount);
    
    contactsCount = newContactsCount;
    constraintsCount = newConstraintsCount;
    variablesCount = newVariablesCount;
239 240
}

241 242
void ConstraintsInfo::clear()
{
243 244 245 246 247 248 249 250 251 252 253
    jacobian.setZero();
    jacobianDerivative.setZero();
    if(inverseMass.size()>0)
    {
      inverseMass.setZero();
    }
    if(forceMin.size()>0)
    {
      forceMin.fill(-HUGE_VAL);
      forceMax.fill(HUGE_VAL);
    }
254 255 256 257

    collisionFlag = false;
}

258
World::World()
259
    : _time(0), _timeScale(1), _errorsCalculation(false),
260
      _solver(NULL), _collisionSolver(NULL), _constraintSolver(NULL),
261
      _variablesCount(0)
262
{
263
    setColor(0xffffffff);
264
    setWorld(this);
265 266 267
    clear();
}

268
World::World(const World& world)
269
    : ItemGroup(), _time(0), _timeScale(1), _errorsCalculation(false),
270
      _solver(NULL), _collisionSolver(NULL), _constraintSolver(NULL),
271
      _variablesCount(0)
272 273 274 275
{
    *this = world;
}

276 277 278 279 280 281 282
World::~World()
{
    clear();
}

void World::clear()
{
283
    // Avoid erasing each element individually in the cache
284 285
    if(_collisionSolver) _collisionSolver->resetCaches();

286 287
    // clear _items
    ItemGroup::clear();
288

289 290
    STEPCORE_ASSERT_NOABORT(_bodies.empty());
    STEPCORE_ASSERT_NOABORT(_forces.empty());
291
    STEPCORE_ASSERT_NOABORT(_joints.empty());
292 293
    //_bodies.clear();
    //_forces.clear();
294

295 296
    delete _solver; _solver = NULL;
    delete _collisionSolver; _collisionSolver = NULL;
297 298
    delete _constraintSolver; _constraintSolver = NULL;

299
    _variablesCount = 0;
300 301
    _variables.resize(0);
    _variances.resize(0);
302
    _tempArray.resize(0);
303

304
    _constraintsInfo.setDimension(0, 0);
305

306 307 308
    setColor(0xffffffff);
    deleteObjectErrors();

309
    _time = 0;
Vladimir Kuznetsov's avatar
Vladimir Kuznetsov committed
310
    _timeScale = 1;
311
    _errorsCalculation = false;
312

313
    _stopOnCollision = false;
314
    _stopOnIntersection = false;
315
    _evolveAbort = false;
316

317
#ifdef STEPCORE_WITH_QT
318
    setName(QString());
319 320 321
#endif
}

322
World& World::operator=(const World& world)
323
{
324 325 326 327 328 329 330 331 332 333 334 335 336 337 338
    if(this == &world) return *this;

    clear();
    ItemGroup::operator=(world);

    if(world._solver) {
        setSolver(static_cast<Solver*>(
                world._solver->metaObject()->cloneObject(*(world._solver))));
    } else setSolver(0);

    if(world._collisionSolver) {
        setCollisionSolver(static_cast<CollisionSolver*>(
               world._collisionSolver->metaObject()->cloneObject(*(world._collisionSolver))));
    } else setCollisionSolver(0);

339
    if(world._constraintSolver) setConstraintSolver(static_cast<ConstraintSolver*>(
340
               world._constraintSolver->metaObject()->cloneObject(*(world._constraintSolver))));
341
    else setConstraintSolver(0);
342 343 344 345 346 347 348 349 350 351 352 353 354 355 356

    _time = world._time;
    _timeScale = world._timeScale;
    _errorsCalculation = world._errorsCalculation;

    _stopOnCollision = world._stopOnCollision;
    _stopOnIntersection = world._stopOnIntersection;
    _evolveAbort = world._evolveAbort;

    // Fix links
    QHash<const Object*, Object*> copyMap;
    copyMap.insert(NULL, NULL);
    copyMap.insert(&world, this);
    if(_solver) copyMap.insert(world._solver, _solver);
    if(_collisionSolver) copyMap.insert(world._collisionSolver, _collisionSolver);
357
    if(_constraintSolver) copyMap.insert(world._constraintSolver, _constraintSolver);
358 359 360 361 362
    fillCopyMap(&copyMap, &world, this);

    applyCopyMap(&copyMap, this);
    if(_solver) applyCopyMap(&copyMap, _solver);
    if(_collisionSolver) applyCopyMap(&copyMap, _collisionSolver);
363
    if(_constraintSolver) applyCopyMap(&copyMap, _constraintSolver);
364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406

    const ItemList::const_iterator end = items().end();
    for(ItemList::const_iterator it = items().begin(); it != end; ++it) {
        worldItemCopied(&copyMap, *it);
    }

    setWorld(this);

    checkVariablesCount();

    return *this;
}

void World::fillCopyMap(QHash<const Object*, Object*>* map,
                        const ItemGroup* g1, ItemGroup* g2)
{
    const ItemList::const_iterator end = g1->items().end();
    for(ItemList::const_iterator it1 = g1->items().begin(),
                                 it2 = g2->items().begin();
                                 it1 != end; ++it1, ++it2) {
        map->insert(*it1, *it2);
        if((*it1)->metaObject()->inherits<StepCore::ItemGroup>())
            fillCopyMap(map, static_cast<ItemGroup*>(*it1), static_cast<ItemGroup*>(*it2));
    }
}

void World::applyCopyMap(QHash<const Object*, Object*>* map, Object* obj)
{
    const StepCore::MetaObject* mobj = obj->metaObject();
    for(int i=0; i<mobj->propertyCount(); ++i) {
        const StepCore::MetaProperty* pr = mobj->property(i);
        if(pr->userTypeId() == qMetaTypeId<Object*>()) {
            QVariant v = pr->readVariant(obj);
            v = QVariant::fromValue(map->value(v.value<Object*>(), NULL));
            pr->writeVariant(obj, v);
        }
    }
}

void World::worldItemCopied(QHash<const Object*, Object*>* map, Item* item)
{
    applyCopyMap(map, item);

407 408
    if(item->metaObject()->inherits<Force>())
        _forces.push_back(dynamic_cast<Force*>(item));
409 410
    if(item->metaObject()->inherits<Joint>())
        _joints.push_back(dynamic_cast<Joint*>(item));
411 412
    if(item->metaObject()->inherits<Body>())
        _bodies.push_back(dynamic_cast<Body*>(item));
413

414 415 416 417
    if(item->metaObject()->inherits<ItemGroup>()) {
        ItemGroup* group = static_cast<ItemGroup*>(item);
        ItemList::const_iterator end = group->items().end();
        for(ItemList::const_iterator it = group->items().begin(); it != end; ++it) {
418
            worldItemCopied(map, *it);
419 420 421 422 423 424 425 426 427
        }
    }
}

void World::worldItemAdded(Item* item)
{
    if(item->metaObject()->inherits<Force>())
        _forces.push_back(dynamic_cast<Force*>(item));

428 429 430
    if(item->metaObject()->inherits<Joint>())
        _joints.push_back(dynamic_cast<Joint*>(item));

431 432 433 434 435 436
    if(item->metaObject()->inherits<Body>()) {
        Body* body = dynamic_cast<Body*>(item);
        _bodies.push_back(body);
        if(_collisionSolver) _collisionSolver->bodyAdded(_bodies, body);
    }

437 438
    if(item->metaObject()->inherits<ItemGroup>()) {
        ItemGroup* group = static_cast<ItemGroup*>(item);
439 440 441 442 443
        ItemList::const_iterator end = group->items().end();
        for(ItemList::const_iterator it = group->items().begin(); it != end; ++it) {
            worldItemAdded(*it);
        }
    }
444 445

    checkVariablesCount();
446 447 448 449
}

void World::worldItemRemoved(Item* item)
{
450 451
    if(item->metaObject()->inherits<ItemGroup>()) {
        ItemGroup* group = static_cast<ItemGroup*>(item);
452 453 454 455 456 457 458 459 460 461 462
        ItemList::const_iterator end = group->items().end();
        for(ItemList::const_iterator it = group->items().begin(); it != end; ++it) {
            worldItemRemoved(*it);
        }
    }

    const ItemList::const_iterator end = items().end();
    for(ItemList::const_iterator it = items().begin(); it != end; ++it) {
        (*it)->worldItemRemoved(item);
    }

463 464 465 466 467 468 469 470
    if(item->metaObject()->inherits<Body>()) {
        Body* body = dynamic_cast<Body*>(item);
        if(_collisionSolver) _collisionSolver->bodyRemoved(_bodies, body);
        BodyList::iterator b = std::find(_bodies.begin(), _bodies.end(), body);
        STEPCORE_ASSERT_NOABORT(b != _bodies.end());
        _bodies.erase(b);
    }

471 472 473 474 475 476 477
    if(item->metaObject()->inherits<Joint>()) {
        JointList::iterator j = std::find(_joints.begin(), _joints.end(),
                                            dynamic_cast<Joint*>(item));
        STEPCORE_ASSERT_NOABORT(j != _joints.end());
        _joints.erase(j);
    }

478 479 480
    if(item->metaObject()->inherits<Force>()) {
        ForceList::iterator f = std::find(_forces.begin(), _forces.end(),
                                            dynamic_cast<Force*>(item));
481 482 483
        STEPCORE_ASSERT_NOABORT(f != _forces.end());
        _forces.erase(f);
    }
484 485 486

    // XXX: on ItemGroup::clear this will be called on each object !
    checkVariablesCount();
487 488 489
}

/*
490 491 492
void World::addItem(Item* item)
{
    _items.push_back(item);
493
    item->setWorld(this);
494 495 496 497 498
    Force* force = dynamic_cast<Force*>(item);
    if(force) _forces.push_back(force);
    Body* body = dynamic_cast<Body*>(item);
    if(body) _bodies.push_back(body);
}
499
*/
500

501
/*void World::removeItem(Item* item)
502
{
Carsten Niehaus's avatar
Carsten Niehaus committed
503 504
    const ItemList::const_iterator it_end = _items.end();
    for(ItemList::iterator it = _items.begin(); it != it_end; ++it)
505
        (*it)->worldItemRemoved(item);
506

507 508
    item->setWorld(NULL);

509 510 511 512 513 514 515 516 517 518 519 520 521 522 523 524 525
    ItemList::iterator i = std::find(_items.begin(), _items.end(), item);
    STEPCORE_ASSERT_NOABORT(i != _items.end());
    _items.erase(i);

    Force* force = dynamic_cast<Force*>(item);
    if(force) {
        ForceList::iterator f = std::find(_forces.begin(), _forces.end(), force);
        STEPCORE_ASSERT_NOABORT(f != _forces.end());
        _forces.erase(f);
    }

    Body* body = dynamic_cast<Body*>(item);
    if(body) {
        BodyList::iterator b = std::find(_bodies.begin(), _bodies.end(), body);
        STEPCORE_ASSERT_NOABORT(b != _bodies.end());
        _bodies.erase(b);
    }
526
}
527
*/
528

529
/*
530 531 532 533 534 535
int World::itemIndex(const Item* item) const
{
    ItemList::const_iterator o = std::find(_items.begin(), _items.end(), item);
    STEPCORE_ASSERT_NOABORT(o != _items.end());
    return std::distance(_items.begin(), o);
}
536
*/
537

538
/*
539 540 541 542 543 544 545
Item* World::item(const QString& name) const
{
    for(ItemList::const_iterator o = _items.begin(); o != _items.end(); ++o) {
        if((*o)->name() == name) return *o;
    }
    return NULL;
}
546
*/
547 548 549

Object* World::object(const QString& name)
{
550
    if(name.isEmpty()) return NULL;
551 552 553
    if(this->name() == name) return this;
    else if(_solver && _solver->name() == name) return _solver;
    else if(_collisionSolver && _collisionSolver->name() == name) return _collisionSolver;
554
    else if(_constraintSolver && _constraintSolver->name() == name) return _constraintSolver;
555 556 557
    else return item(name);
}

558 559 560 561 562
void World::setSolver(Solver* solver)
{
    delete _solver;
    _solver = solver;
    if(_solver != 0) {
563
        _solver->setDimension(_variablesCount*2);
564 565 566 567 568
        _solver->setFunction(solverFunction);
        _solver->setParams(this);
    }
}

569 570 571 572 573 574 575
Solver* World::removeSolver()
{
    Solver* solver = _solver;
    _solver = NULL;
    return solver;
}

576
void World::setCollisionSolver(CollisionSolver* collisionSolver)
577
{
578 579
    delete _collisionSolver;
    _collisionSolver = collisionSolver;
580 581
}

582
CollisionSolver* World::removeCollisionSolver()
583
{
584 585 586
    CollisionSolver* collisionSolver = _collisionSolver;
    _collisionSolver = NULL;
    return collisionSolver;
587 588
}

589 590 591 592 593 594 595 596 597 598 599 600 601
void World::setConstraintSolver(ConstraintSolver* constraintSolver)
{
    delete _constraintSolver;
    _constraintSolver = constraintSolver;
}

ConstraintSolver* World::removeConstraintSolver()
{
    ConstraintSolver* constraintSolver = _constraintSolver;
    _constraintSolver = NULL;
    return constraintSolver;
}

602 603 604 605
void World::checkVariablesCount()
{
    int variablesCount = 0;
    for(BodyList::iterator b = _bodies.begin(); b != _bodies.end(); ++b) {
606
        (*b)->setVariablesOffset(variablesCount);
607 608
        variablesCount += (*b)->variablesCount();
    }
609 610 611 612 613

    int constraintsCount = 0;
    for(JointList::iterator j = _joints.begin(); j != _joints.end(); ++j) {
        constraintsCount += (*j)->constraintsCount();
    }
614 615
    
    _constraintsInfo.setDimension(variablesCount, constraintsCount, 0);
616 617 618 619 620

    if(variablesCount != _variablesCount) {
        _variablesCount = variablesCount;
        _variables.resize(_variablesCount*2);
        _variances.resize(_variablesCount*2);
621
        _tempArray.resize(_variablesCount*2);
622
        if(_solver) _solver->setDimension(_variablesCount*2);
623
    }
624 625
}

626
void World::gatherAccelerations(double* acceleration, double* accelerationVariance)
627
{
628 629 630
    if(accelerationVariance)
        memset(accelerationVariance, 0, _variablesCount*sizeof(*accelerationVariance));

631
    int index = 0;
Carsten Niehaus's avatar
Carsten Niehaus committed
632 633
    const BodyList::const_iterator it_end = _bodies.end();
    for(BodyList::iterator b = _bodies.begin(); b != it_end; ++b) {
634
        (*b)->getAccelerations(acceleration + index, accelerationVariance ? accelerationVariance + index : NULL);
635 636 637 638
        index += (*b)->variablesCount();
    }
}

639
void World::gatherVariables(double* variables, double* variances)
640
{
641 642
    if(variances) memset(variances, 0, _variablesCount*2*sizeof(*variances));

643
    int index = 0;
Carsten Niehaus's avatar
Carsten Niehaus committed
644 645
    const BodyList::const_iterator it_end = _bodies.end();
    for(BodyList::iterator b = _bodies.begin(); b != it_end; ++b) {
646 647 648
        (*b)->getVariables(variables + index, variables + _variablesCount + index,
                           variances ? variances + index : NULL,
                           variances ? variances + _variablesCount + index : NULL);
649 650 651 652
        index += (*b)->variablesCount();
    }
}

653
void World::scatterVariables(const double* variables, const double* variances)
654 655
{
    int index = 0;
Carsten Niehaus's avatar
Carsten Niehaus committed
656 657
    const BodyList::const_iterator it_end = _bodies.end();
    for(BodyList::iterator b = _bodies.begin(); b != it_end; ++b) {
658 659 660
        (*b)->setVariables(variables + index,  variables + _variablesCount + index,
                           variances ? variances + index : NULL,
                           variances ? variances + _variablesCount + index : NULL);
661 662 663 664
        index += (*b)->variablesCount();
    }
}

665 666 667 668 669 670 671 672 673 674 675 676 677 678 679 680 681 682 683
void World::gatherJointsInfo(ConstraintsInfo* info)
{
    info->clear();

    int offset = 0;
    const BodyList::const_iterator b_end = _bodies.end();
    for(BodyList::iterator body = _bodies.begin(); body != b_end; ++body) {
        (*body)->getInverseMass(&(info->inverseMass), NULL, offset);
        offset += (*body)->variablesCount();
    }

    offset = 0;
    const JointList::const_iterator j_end = _joints.end();
    for(JointList::iterator joint = _joints.begin(); joint != j_end; ++joint) {
        (*joint)->getConstraintsInfo(info, offset);
        offset += (*joint)->constraintsCount();
    }
}

Vladimir Kuznetsov's avatar
Vladimir Kuznetsov committed
684 685 686 687
int World::doCalcFn()
{
    STEPCORE_ASSERT_NOABORT(_solver != NULL);

688
    //if(_collisionSolver) _collisionSolver->resetCaches();
689

690
    _stopOnCollision = false;
691
    _stopOnIntersection = false;
Vladimir Kuznetsov's avatar
Vladimir Kuznetsov committed
692
    checkVariablesCount();
693 694 695
    gatherVariables(_variables.data(), _errorsCalculation ? _variances.data() : NULL);
    return _solver->doCalcFn(&_time, &_variables, _errorsCalculation ? &_variances : NULL,
                             NULL, _errorsCalculation ? &_variances : NULL);
Vladimir Kuznetsov's avatar
Vladimir Kuznetsov committed
696 697 698 699 700
}

int World::doEvolve(double delta)
{
    STEPCORE_ASSERT_NOABORT(_solver != NULL);
701
    
Vladimir Kuznetsov's avatar
Vladimir Kuznetsov committed
702
    checkVariablesCount();
703
    gatherVariables(_variables.data(), _errorsCalculation ? _variances.data() : NULL);
Vladimir Kuznetsov's avatar
Vladimir Kuznetsov committed
704

705 706 707
    int ret = Solver::OK;
    double targetTime = _time + delta*_timeScale;
    
708
    if(_collisionSolver) {
709
        //_collisionSolver->resetCaches();
710
        if(Contact::Intersected == _collisionSolver->checkContacts(_bodies))
711 712
            return Solver::IntersectionDetected;
    }
713

714
    while(_time < targetTime) {
715 716 717 718
        STEPCORE_ASSERT_NOABORT( targetTime - _time > _solver->stepSize() / 1000 );
        if( !(   targetTime - _time > _solver->stepSize() / 1000 ) ) {
                    qDebug("* %e %e %e", targetTime, _time, _solver->stepSize());
        }
719
        double time = _time;
720

721
        _stopOnCollision = true;
722
        _stopOnIntersection = true;
723
        
724 725
        ret = _solver->doEvolve(&time, targetTime, &_variables,
                            _errorsCalculation ? &_variances : NULL);
726
        
727 728 729
        _time = time;

        if(ret == Solver::CollisionDetected ||
730
           ret == Solver::IntersectionDetected) {
731 732 733 734 735
            // If we have stopped on collision
            // 1. Decrease timestep to stop before collision
            // 2. Proceed with decresed timestep until
            //    - we have meet collision again: go to 1
            //    - we pass collision point: it means that we have come close enough
736
            //      to collision point and CollisionSolver have resolved collision
737 738
            // We can't simply change Solver::stepSize since adaptive solvers can
            // abuse our settings so we have to step manually
739 740 741
            //STEPCORE_ASSERT_NOABORT(_collisionTime <= targetTime);
            //STEPCORE_ASSERT_NOABORT(_collisionTime > _time);
            double stepSize = fmin(_solver->stepSize() / 2, targetTime - _time);
742 743
            double collisionEndTime = targetTime - _time > stepSize*3.01 ? _time + stepSize*3 : targetTime;

744
            _stopOnCollision = false;
745 746

            do {
747 748
                double endTime = collisionEndTime - time > stepSize*1.01 ? time+stepSize : collisionEndTime;

749 750
                ret = _solver->doEvolve(&time, endTime, &_variables,
                            _errorsCalculation ? &_variances : NULL);
751
                
752 753
                _time = time;

754
                if(ret == Solver::IntersectionDetected || ret == Solver::CollisionDetected) {
755 756 757
                    //STEPCORE_ASSERT_NOABORT(_collisionTime > _time);
                    //STEPCORE_ASSERT_NOABORT(_collisionTime < _collisionExpectedTime);
                    stepSize = fmin(stepSize/2, targetTime - _time);
758 759
                    collisionEndTime = targetTime - _time > stepSize*3.01 ? _time + stepSize*3 : targetTime;

760
                    //STEPCORE_ASSERT_NOABORT(_time + stepSize != _time);
761
                    // XXX: what to do if stepSize becomes too small ?
762

763
                } else if(ret == Solver::OK) {
764 765 766 767 768 769
                    // We can be close to the collision point.
                    //
                    // Now we will calculate impulses required to fix collisions.
                    // All joints should be taken into account, but since we are
                    // calculating impulses we should "froze" the jacobian i.e.
                    // ignore it derivative
770 771 772 773 774 775 776 777 778 779 780 781 782 783 784 785 786 787
                    scatterVariables(_variables.data(), _errorsCalculation ? _variances.data() : NULL);
                    
                    // check and count contacts before the sparse matrix assembly
                    int contactsCount = 0;
                    int status = _collisionSolver->checkContacts(_bodies, true, &contactsCount);
                    if (contactsCount!=_constraintsInfo.contactsCount)
                    {
                      _constraintsInfo.setDimension(_constraintsInfo.variablesCount,
                                                    _constraintsInfo.constraintsCount,
                                                    contactsCount);
                    }
                    
                    _tempArray.setZero();
                    ::new (&_constraintsInfo.position) MappedVector(_variables.data(), _variablesCount);
                    ::new (&_constraintsInfo.velocity) MappedVector(_variables.data()+_variablesCount, _variablesCount);
                    ::new (&_constraintsInfo.acceleration) MappedVector(_tempArray.data(), _variablesCount);
                    
                    // start sparse matrix assembly
788
                    gatherJointsInfo(&_constraintsInfo);
789 790
                    _constraintsInfo.jacobianDerivative.setZero();
                    
791
                    if(status >= CollisionSolver::InternalError) { ret = status; goto out; }
792 793 794
                    
                    _collisionSolver->getContactsInfo(_constraintsInfo, true);
                    // end sparse matrix assembly
795 796

                    if(_constraintsInfo.collisionFlag) {
797 798
                        ret = _constraintSolver->solve(&_constraintsInfo);
                        if(ret != Solver::OK) goto out;
799 800

                        // XXX: variances
801
                        _constraintsInfo.velocity += _constraintsInfo.inverseMass.asDiagonal() * _constraintsInfo.force;
802
                    }
803 804
                } else goto out;

805
            } while(_time + stepSize/100 <= collisionEndTime); // XXX
806
        } else if(ret != Solver::OK) goto out;
Vladimir Kuznetsov's avatar
Vladimir Kuznetsov committed
807 808
    }

809
out:
810
    scatterVariables(_variables.data(), _errorsCalculation ? _variances.data() : NULL);
Vladimir Kuznetsov's avatar
Vladimir Kuznetsov committed
811 812 813
    return ret;
}

814 815
inline int World::solverFunction(double t, const double* y,
                    const double* yvar, double* f, double* fvar)
816
{
817 818
    if(_evolveAbort) return Solver::Aborted;

819
    _time = t;
820
    scatterVariables(y, yvar); // this will reset force
821

822 823 824 825 826 827
    // 1. Forces
    bool calcVariances = (fvar != NULL);
    const ForceList::const_iterator f_end = _forces.end();
    for(ForceList::iterator force = _forces.begin(); force != f_end; ++force) {
        (*force)->calcForce(calcVariances);
    }
828
    
829 830 831
    std::memcpy(f, y+_variablesCount, _variablesCount*sizeof(*f));
    if(fvar) std::memcpy(fvar, y+_variablesCount, _variablesCount*sizeof(*fvar));
    gatherAccelerations(f+_variablesCount, fvar ? fvar+_variablesCount : NULL);
832 833 834 835 836 837 838 839 840 841 842 843 844 845 846 847 848 849 850 851 852 853 854 855 856 857 858 859 860 861 862 863 864 865 866 867
    
    if (_constraintSolver)
    {
      // setup...
      
      // check contacts
      int state = 0;
      if(_collisionSolver) {
          int contactsCount = 0;
          state = _collisionSolver->checkContacts(_bodies, false, &contactsCount);
          
          if (contactsCount!=_constraintsInfo.contactsCount)
          {
            _constraintsInfo.setDimension(_constraintsInfo.variablesCount,
                                          _constraintsInfo.constraintsCount,
                                          contactsCount);
          }
      }

      if(_variablesCount>0)
      {
        ::new (&_constraintsInfo.position) MappedVector(y, _variablesCount);
        ::new (&_constraintsInfo.velocity) MappedVector(y+_variablesCount, _variablesCount);
        ::new (&_constraintsInfo.acceleration) MappedVector(f+_variablesCount, _variablesCount);
      }

      // end sparse matrix assembly
      
      // 2. Joints
      gatherJointsInfo(&_constraintsInfo);

      // 3. Collisions (TODO: variances for collisions)
      if(_collisionSolver) {
          _collisionSolver->getContactsInfo(_constraintsInfo, false);
          if(state == Contact::Intersected) {
              if(_stopOnIntersection) return Solver::IntersectionDetected;
868 869 870 871 872 873 874 875 876 877 878 879 880 881
          } else {
              if(state == Contact::Colliding) {
                  if(_stopOnCollision) return Solver::CollisionDetected;
                  // XXX: We are not stopping on colliding contact
                  // and resolving them only at the end of timestep
                  // XXX: is it right solution ? Shouldn't we try to find
                  // contact point more exactly for example using binary search ?
                  //_collisionTime = t;
                  //_collisionTime = t;
                  //if(t < _collisionExpectedTime)
                  //    return DantzigLCPCollisionSolver::CollisionDetected;
              } else if(state >= CollisionSolver::InternalError) {
                  return state;
              }
882 883 884 885 886 887 888 889 890 891 892 893 894 895 896 897 898 899
          }
      }
      // end sparse matrix assembly

      // 4. Solve constraints
      if(_constraintsInfo.constraintsCount + _constraintsInfo.contactsCount > 0) {

          int state = _constraintSolver->solve(&_constraintsInfo);
          if(state != Solver::OK) return state;

          int offset = 0;
          const BodyList::const_iterator b_end = _bodies.end();
          for(BodyList::iterator body = _bodies.begin(); body != b_end; ++body) {
              (*body)->addForce(&_constraintsInfo.force[offset], NULL);
              (*body)->getAccelerations(f+_variablesCount+offset, NULL);
              offset += (*body)->variablesCount();
          }
      }
900 901
    }

902 903 904
    return 0;
}

905 906
int World::solverFunction(double t, const double* y,
                const double* yvar, double* f, double* fvar, void* params)
907
{
908
    return static_cast<World*>(params)->solverFunction(t, y, yvar, f, fvar);
909 910 911 912
}

} // namespace StepCore