WebSVN – clockwerk-opensim – Blame – Rev 1 – /OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs

1

vero

1

/*

2

* Copyright (c) Contributors, http://opensimulator.org/

3

* See CONTRIBUTORS.TXT for a full list of copyright holders.

4

*

5

* Redistribution and use in source and binary forms, with or without

6

* modification, are permitted provided that the following conditions are met:

7

* * Redistributions of source code must retain the above copyright

8

* notice, this list of conditions and the following disclaimer.

9

* * Redistributions in binary form must reproduce the above copyrightD

10

* notice, this list of conditions and the following disclaimer in the

11

* documentation and/or other materials provided with the distribution.

12

* * Neither the name of the OpenSimulator Project nor the

13

* names of its contributors may be used to endorse or promote products

14

* derived from this software without specific prior written permission.

15

*

16

* THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY

17

* EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED

18

* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE

19

* DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY

20

* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES

21

* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;

22

* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND

23

* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT

24

* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS

25

* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

26

*/

27

using System;

28

using System.Collections.Generic;

29

using System.Text;

30

31

using OpenSim.Framework;

32

33

using OMV = OpenMetaverse;

34

35

namespace OpenSim.Region.Physics.BulletSPlugin

36

{

37

38

public sealed class BSLinksetCompound : BSLinkset

39

{

40

#pragma warning disable 414

41

private static string LogHeader = "[BULLETSIM LINKSET COMPOUND]";

42

#pragma warning restore 414

43

44

public BSLinksetCompound(BSScene scene, BSPrimLinkable parent)

45

: base(scene, parent)

46

{

47

LinksetImpl = LinksetImplementation.Compound;

48

}

49

50

// ================================================================

51

// Changing the physical property of the linkset only needs to change the root

52

public override void SetPhysicalFriction(float friction)

53

{

54

if (LinksetRoot.PhysBody.HasPhysicalBody)

55

m_physicsScene.PE.SetFriction(LinksetRoot.PhysBody, friction);

56

}

57

public override void SetPhysicalRestitution(float restitution)

58

{

59

if (LinksetRoot.PhysBody.HasPhysicalBody)

60

m_physicsScene.PE.SetRestitution(LinksetRoot.PhysBody, restitution);

61

}

62

public override void SetPhysicalGravity(OMV.Vector3 gravity)

63

{

64

if (LinksetRoot.PhysBody.HasPhysicalBody)

65

m_physicsScene.PE.SetGravity(LinksetRoot.PhysBody, gravity);

66

}

67

public override void ComputeAndSetLocalInertia(OMV.Vector3 inertiaFactor, float linksetMass)

68

{

69

OMV.Vector3 inertia = m_physicsScene.PE.CalculateLocalInertia(LinksetRoot.PhysShape.physShapeInfo, linksetMass);

70

LinksetRoot.Inertia = inertia * inertiaFactor;

71

m_physicsScene.PE.SetMassProps(LinksetRoot.PhysBody, linksetMass, LinksetRoot.Inertia);

72

m_physicsScene.PE.UpdateInertiaTensor(LinksetRoot.PhysBody);

73

}

74

public override void SetPhysicalCollisionFlags(CollisionFlags collFlags)

75

{

76

if (LinksetRoot.PhysBody.HasPhysicalBody)

77

m_physicsScene.PE.SetCollisionFlags(LinksetRoot.PhysBody, collFlags);

78

}

79

public override void AddToPhysicalCollisionFlags(CollisionFlags collFlags)

80

{

81

if (LinksetRoot.PhysBody.HasPhysicalBody)

82

m_physicsScene.PE.AddToCollisionFlags(LinksetRoot.PhysBody, collFlags);

83

}

84

public override void RemoveFromPhysicalCollisionFlags(CollisionFlags collFlags)

85

{

86

if (LinksetRoot.PhysBody.HasPhysicalBody)

87

m_physicsScene.PE.RemoveFromCollisionFlags(LinksetRoot.PhysBody, collFlags);

88

}

89

// ================================================================

90

91

// When physical properties are changed the linkset needs to recalculate

92

// its internal properties.

93

public override void Refresh(BSPrimLinkable requestor)

94

{

95

// Something changed so do the rebuilding thing

96

ScheduleRebuild(requestor);

97

base.Refresh(requestor);

98

}

99

100

// Schedule a refresh to happen after all the other taint processing.

101

private void ScheduleRebuild(BSPrimLinkable requestor)

102

{

103

DetailLog("{0},BSLinksetCompound.ScheduleRebuild,,rebuilding={1},hasChildren={2},actuallyScheduling={3}",

104

requestor.LocalID, Rebuilding, HasAnyChildren, (!Rebuilding && HasAnyChildren));

105

106

// When rebuilding, it is possible to set properties that would normally require a rebuild.

107

// If already rebuilding, don't request another rebuild.

108

// If a linkset with just a root prim (simple non-linked prim) don't bother rebuilding.

109

if (!Rebuilding && HasAnyChildren)

110

{

111

m_physicsScene.PostTaintObject("BSLinksetCompound.ScheduleRebuild", LinksetRoot.LocalID, delegate()

112

{

113

if (HasAnyChildren)

114

RecomputeLinksetCompound();

});

}

}

// The object is going dynamic (physical). Do any setup necessary for a dynamic linkset.

120

// Only the state of the passed object can be modified. The rest of the linkset

121

// has not yet been fully constructed.

122

// Return 'true' if any properties updated on the passed object.

123

// Called at taint-time!

124

public override bool MakeDynamic(BSPrimLinkable child)

125

{

126

bool ret = false;

127

DetailLog("{0},BSLinksetCompound.MakeDynamic,call,IsRoot={1}", child.LocalID, IsRoot(child));

128

if (IsRoot(child))

129

{

130

// The root is going dynamic. Rebuild the linkset so parts and mass get computed properly.

131

Refresh(LinksetRoot);

}

return ret;

}

// The object is going static (non-physical). We do not do anything for static linksets.

137

// Return 'true' if any properties updated on the passed object.

138

// Called at taint-time!

139

public override bool MakeStatic(BSPrimLinkable child)

{

bool ret = false;

DetailLog("{0},BSLinksetCompound.MakeStatic,call,IsRoot={1}", child.LocalID, IsRoot(child));

144

child.ClearDisplacement();

145

if (IsRoot(child))

146

{

147

// Schedule a rebuild to verify that the root shape is set to the real shape.

148

Refresh(LinksetRoot);

}

return ret;

}

// 'physicalUpdate' is true if these changes came directly from the physics engine. Don't need to rebuild then.

154

// Called at taint-time.

155

public override void UpdateProperties(UpdatedProperties whichUpdated, BSPrimLinkable updated)

156

{

157

if (!LinksetRoot.IsPhysicallyActive)

158

{

159

// No reason to do this physical stuff for static linksets.

160

DetailLog("{0},BSLinksetCompound.UpdateProperties,notPhysical", LinksetRoot.LocalID);

return;

}

// The user moving a child around requires the rebuilding of the linkset compound shape

165

// One problem is this happens when a border is crossed -- the simulator implementation

166

// stores the position into the group which causes the move of the object

167

// but it also means all the child positions get updated.

168

// What would cause an unnecessary rebuild so we make sure the linkset is in a

169

// region before bothering to do a rebuild.

170

if (!IsRoot(updated) && m_physicsScene.TerrainManager.IsWithinKnownTerrain(LinksetRoot.RawPosition))

171

{

172

// If a child of the linkset is updating only the position or rotation, that can be done

173

// without rebuilding the linkset.

174

// If a handle for the child can be fetch, we update the child here. If a rebuild was

175

// scheduled by someone else, the rebuild will just replace this setting.

176

177

bool updatedChild = false;

178

// Anything other than updating position or orientation usually means a physical update

179

// and that is caused by us updating the object.

180

if ((whichUpdated & ~(UpdatedProperties.Position | UpdatedProperties.Orientation)) == 0)

181

{

182

// Find the physical instance of the child

183

                if (LinksetRoot.PhysShape.HasPhysicalShape && m_physicsScene.PE.IsCompound(LinksetRoot.PhysShape.physShapeInfo))

184

{

185

// It is possible that the linkset is still under construction and the child is not yet

186

// inserted into the compound shape. A rebuild of the linkset in a pre-step action will

187

// build the whole thing with the new position or rotation.

188

// The index must be checked because Bullet references the child array but does no validity

189

// checking of the child index passed.

190

                    int numLinksetChildren = m_physicsScene.PE.GetNumberOfCompoundChildren(LinksetRoot.PhysShape.physShapeInfo);

191

if (updated.LinksetChildIndex < numLinksetChildren)

192

{

193

                        BulletShape linksetChildShape = m_physicsScene.PE.GetChildShapeFromCompoundShapeIndex(LinksetRoot.PhysShape.physShapeInfo, updated.LinksetChildIndex);

194

if (linksetChildShape.HasPhysicalShape)

195

{

196

// Found the child shape within the compound shape

197

                            m_physicsScene.PE.UpdateChildTransform(LinksetRoot.PhysShape.physShapeInfo, updated.LinksetChildIndex,

198

updated.RawPosition - LinksetRoot.RawPosition,

199

                                                                        updated.RawOrientation * OMV.Quaternion.Inverse(LinksetRoot.RawOrientation),

200

true /* shouldRecalculateLocalAabb */);

201

updatedChild = true;

202

                            DetailLog("{0},BSLinksetCompound.UpdateProperties,changeChildPosRot,whichUpdated={1},pos={2},rot={3}",

203

                                                        updated.LocalID, whichUpdated, updated.RawPosition, updated.RawOrientation);

204

}

205

else // DEBUG DEBUG

206

{ // DEBUG DEBUG

207

                            DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,noChildShape,shape={1}",

208

updated.LocalID, linksetChildShape);

209

} // DEBUG DEBUG

210

}

211

else // DEBUG DEBUG

212

{ // DEBUG DEBUG

213

// the child is not yet in the compound shape. This is non-fatal.

214

                        DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,childNotInCompoundShape,numChildren={1},index={2}",

215

                                                                    updated.LocalID, numLinksetChildren, updated.LinksetChildIndex);

216

} // DEBUG DEBUG

217

}

218

else // DEBUG DEBUG

219

{ // DEBUG DEBUG

220

                    DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,noBodyOrNotCompound", updated.LocalID);

} // DEBUG DEBUG

if (!updatedChild)

{

                    // If couldn't do the individual child, the linkset needs a rebuild to incorporate the new child info.

226

// Note: there are several ways through this code that will not update the child if

227

// the linkset is being rebuilt. In this case, scheduling a rebuild is a NOOP since

228

// there will already be a rebuild scheduled.

229

                    DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild.schedulingRebuild,whichUpdated={1}",

230

updated.LocalID, whichUpdated);

Refresh(updated);

}

}

}

}

// Routine called when rebuilding the body of some member of the linkset.

238

// If one of the bodies is being changed, the linkset needs rebuilding.

239

// For instance, a linkset is built and then a mesh asset is read in and the mesh is recreated.

240

// Returns 'true' of something was actually removed and would need restoring

241

// Called at taint-time!!

242

public override bool RemoveDependencies(BSPrimLinkable child)

{

bool ret = false;

DetailLog("{0},BSLinksetCompound.RemoveDependencies,refreshIfChild,rID={1},rBody={2},isRoot={3}",

247

child.LocalID, LinksetRoot.LocalID, LinksetRoot.PhysBody, IsRoot(child));

Refresh(child);

return ret;

}

// ================================================================

255

256

// Add a new child to the linkset.

257

// Called while LinkActivity is locked.

258

protected override void AddChildToLinkset(BSPrimLinkable child)

259

{

260

if (!HasChild(child))

261

{

262

m_children.Add(child, new BSLinkInfo(child));

263

264

DetailLog("{0},BSLinksetCompound.AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID);

265

266

// Rebuild the compound shape with the new child shape included

Refresh(child);

}

return;

}

// Remove the specified child from the linkset.

273

// Safe to call even if the child is not really in the linkset.

274

protected override void RemoveChildFromLinkset(BSPrimLinkable child, bool inTaintTime)

275

{

276

child.ClearDisplacement();

277

278

if (m_children.Remove(child))

279

{

280

DetailLog("{0},BSLinksetCompound.RemoveChildFromLinkset,call,rID={1},rBody={2},cID={3},cBody={4}",

281

child.LocalID,

282

LinksetRoot.LocalID, LinksetRoot.PhysBody.AddrString,

283

child.LocalID, child.PhysBody.AddrString);

284

285

// Cause the child's body to be rebuilt and thus restored to normal operation

286

child.ForceBodyShapeRebuild(inTaintTime);

287

288

if (!HasAnyChildren)

289

{

290

// The linkset is now empty. The root needs rebuilding.

291

LinksetRoot.ForceBodyShapeRebuild(inTaintTime);

}

else

{

// Rebuild the compound shape with the child removed

296

Refresh(LinksetRoot);

}

}

return;

}

// Called before the simulation step to make sure the compound based linkset

303

// is all initialized.

304

// Constraint linksets are rebuilt every time.

305

// Note that this works for rebuilding just the root after a linkset is taken apart.

306

// Called at taint time!!

307

    private bool UseBulletSimRootOffsetHack = false;    // Attempt to have Bullet track the coords of root compound shape

308

// Number of times to perform rebuilds on broken linkset children. This should only happen when

309

// a linkset is initially being created and should happen only one or two times at the most.

310

// This exists to cause a looping problem to be reported while not rebuilding a linkset forever.

311

private static int LinksetRebuildFailureLoopPrevention = 10;

312

private void RecomputeLinksetCompound()

{

try

{

Rebuilding = true;

// No matter what is being done, force the root prim's PhysBody and PhysShape to get set

319

// to what they should be as if the root was not in a linkset.

320

// Not that bad since we only get into this routine if there are children in the linkset and

321

// something has been updated/changed.

322

// Have to do the rebuild before checking for physical because this might be a linkset

323

// being destructed and going non-physical.

324

LinksetRoot.ForceBodyShapeRebuild(true);

325

326

// There is no reason to build all this physical stuff for a non-physical or empty linkset.

327

if (!LinksetRoot.IsPhysicallyActive || !HasAnyChildren)

328

{

329

                DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,notPhysicalOrNoChildren", LinksetRoot.LocalID);

330

return; // Note the 'finally' clause at the botton which will get executed.

331

}

332

333

// Get a new compound shape to build the linkset shape in.

334

BSShape linksetShape = BSShapeCompound.GetReference(m_physicsScene);

335

336

// Compute a displacement for each component so it is relative to the center-of-mass.

337

// Bullet presumes an object's origin (relative <0,0,0>) is its center-of-mass

338

OMV.Vector3 centerOfMassW = ComputeLinksetCenterOfMass();

339

340

            OMV.Quaternion invRootOrientation = OMV.Quaternion.Normalize(OMV.Quaternion.Inverse(LinksetRoot.RawOrientation));

341

OMV.Vector3 origRootPosition = LinksetRoot.RawPosition;

342

343

            // 'centerDisplacementV' is the vehicle relative distance from the simulator root position to the center-of-mass

344

OMV.Vector3 centerDisplacementV = (centerOfMassW - LinksetRoot.RawPosition) * invRootOrientation;

345

if (UseBulletSimRootOffsetHack || !BSParam.LinksetOffsetCenterOfMass)

346

{

347

// Zero everything if center-of-mass displacement is not being done.

348

centerDisplacementV = OMV.Vector3.Zero;

349

LinksetRoot.ClearDisplacement();

}

else

{

// The actual center-of-mass could have been set by the user.

354

centerDisplacementV = LinksetRoot.SetEffectiveCenterOfMassDisplacement(centerDisplacementV);

355

}

356

357

DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,COM,rootPos={1},com={2},comDisp={3}",

358

LinksetRoot.LocalID, origRootPosition, centerOfMassW, centerDisplacementV);

359

360

// Add the shapes of all the components of the linkset

361

int memberIndex = 1;

362

ForEachMember((cPrim) =>

{

if (IsRoot(cPrim))

{

// Root shape is always index zero.

367

cPrim.LinksetChildIndex = 0;

}

else

{

cPrim.LinksetChildIndex = memberIndex;

memberIndex++;

}

// Get a reference to the shape of the child for adding of that shape to the linkset compound shape

376

BSShape childShape = cPrim.PhysShape.GetReference(m_physicsScene, cPrim);

377

378

// Offset the child shape from the center-of-mass and rotate it to vehicle relative.

379

                OMV.Vector3 offsetPos = (cPrim.RawPosition - origRootPosition) * invRootOrientation - centerDisplacementV;

380

OMV.Quaternion offsetRot = OMV.Quaternion.Normalize(cPrim.RawOrientation) * invRootOrientation;

381

382

// Add the child shape to the compound shape being built

383

if (childShape.physShapeInfo.HasPhysicalShape)

384

{

385

                    m_physicsScene.PE.AddChildShapeToCompoundShape(linksetShape.physShapeInfo, childShape.physShapeInfo, offsetPos, offsetRot);

386

                    DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addChild,indx={1},cShape={2},offPos={3},offRot={4}",

387

LinksetRoot.LocalID, cPrim.LinksetChildIndex, childShape, offsetPos, offsetRot);

388

389

// Since we are borrowing the shape of the child, disable the origional child body

390

if (!IsRoot(cPrim))

391

{

392

m_physicsScene.PE.AddToCollisionFlags(cPrim.PhysBody, CollisionFlags.CF_NO_CONTACT_RESPONSE);

393

m_physicsScene.PE.ForceActivationState(cPrim.PhysBody, ActivationState.DISABLE_SIMULATION);

394

// We don't want collisions from the old linkset children.

395

                        m_physicsScene.PE.RemoveFromCollisionFlags(cPrim.PhysBody, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);

396

cPrim.PhysBody.collisionType = CollisionType.LinksetChild;

}

}

else

{

// The linkset must be in an intermediate state where all the children have not yet

402

// been constructed. This sometimes happens on startup when everything is getting

403

// built and some shapes have to wait for assets to be read in.

404

// Just skip this linkset for the moment and cause the shape to be rebuilt next tick.

405

// One problem might be that the shape is broken somehow and it never becomes completely

406

// available. This might cause the rebuild to happen over and over.

407

LinksetRoot.ForceBodyShapeRebuild(false);

408

                    DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addChildWithNoShape,indx={1},cShape={2},offPos={3},offRot={4}",

409

LinksetRoot.LocalID, cPrim.LinksetChildIndex, childShape, offsetPos, offsetRot);

410

// Output an annoying warning. It should only happen once but if it keeps coming out,

411

// the user knows there is something wrong and will report it.

412

                    m_physicsScene.Logger.WarnFormat("{0} Linkset rebuild warning. If this happens more than one or two times, please report in Mantis 7191", LogHeader);

413

m_physicsScene.Logger.WarnFormat("{0} pName={1}, childIdx={2}, shape={3}",

414

LogHeader, LinksetRoot.Name, cPrim.LinksetChildIndex, childShape);

415

416

// This causes the loop to bail on building the rest of this linkset.

417

// The rebuild operation should fix it up or declare the object unbuildable.

return true;

}

return false; // 'false' says to move onto the next child in the list

422

});

423

424

// Replace the root shape with the built compound shape.

425

// Object removed and added to world to get collision cache rebuilt for new shape.

426

LinksetRoot.PhysShape.Dereference(m_physicsScene);

427

LinksetRoot.PhysShape = linksetShape;

428

m_physicsScene.PE.RemoveObjectFromWorld(m_physicsScene.World, LinksetRoot.PhysBody);

429

m_physicsScene.PE.SetCollisionShape(m_physicsScene.World, LinksetRoot.PhysBody, linksetShape.physShapeInfo);

430

m_physicsScene.PE.AddObjectToWorld(m_physicsScene.World, LinksetRoot.PhysBody);

431

DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addBody,body={1},shape={2}",

432

LinksetRoot.LocalID, LinksetRoot.PhysBody, linksetShape);

433

434

// With all of the linkset packed into the root prim, it has the mass of everyone.

435

LinksetMass = ComputeLinksetMass();

436

LinksetRoot.UpdatePhysicalMassProperties(LinksetMass, true);

437

438

if (UseBulletSimRootOffsetHack)

439

{

440

// Enable the physical position updator to return the position and rotation of the root shape.

441

// This enables a feature in the C++ code to return the world coordinates of the first shape in the

442

// compound shape. This aleviates the need to offset the returned physical position by the

443

// center-of-mass offset.

444

// TODO: either debug this feature or remove it.

445

                m_physicsScene.PE.AddToCollisionFlags(LinksetRoot.PhysBody, CollisionFlags.BS_RETURN_ROOT_COMPOUND_SHAPE);

}

}

finally

{

Rebuilding = false;

451

}

452

453

// See that the Aabb surrounds the new shape

454

m_physicsScene.PE.RecalculateCompoundShapeLocalAabb(LinksetRoot.PhysShape.physShapeInfo);

455

}

456

}

457

}

clockwerk-opensim – Blame information for rev 1