bool intersectTwoRangeEdgesAndPoint(Particle particle, int re0, int re1, int pt, ref ParticleContact contact)
{
ParticleContact c0 = default(ParticleContact), c1 = default(ParticleContact);
if (m_rangeEdges[re0].intersect(particle, ref c0))
{
if (m_rangeEdges[re1].intersect(particle, ref c1))
{
if (c0.penetration < c1.penetration)
{
contact = c0;
setDoorSide(re0, ref contact);
}
else
{
contact = c1;
setDoorSide(re1, ref contact);
}
}
else
{
contact = c0;
setDoorSide(re0, ref contact);
}
return(true);
}
if (m_rangeEdges[re1].intersect(particle, ref contact))
{
setDoorSide(re1, ref contact);
return(true);
}
return(particle.intersect(m_points[pt], ref contact));
}
public void TestConstructor()
{
var p = new List <Particle>
{
new Particle
{
Position = new Vector2D(0, 0),
Velocity = new Vector2D(1, 0),
Mass = 1
},
new Particle
{
Position = new Vector2D(2, 0),
Velocity = new Vector2D(0, 0),
Mass = 1
}
};
var contact = new ParticleContact(p[0], p[1], 0.5, 1, new Vector2D(-1, 0));
Assert.AreEqual(p[0], contact.PA, "PA");
Assert.AreEqual(p[1], contact.PB, "PB");
Assert.AreEqual(0.5, contact.Restitution, "Restitution");
Assert.AreEqual(1, contact.Penetration, "Penetration");
Assert.AreEqual(new Vector2D(-1, 0), contact.ContactNormal, "Contact Normal");
}
/// <summary>
/// 根据碰撞类型为两个指定的形状执行碰撞检测
/// </summary>
/// <param name="type"></param>
/// <param name="sharpA"></param>
/// <param name="sharpB"></param>
public static ParticleContact DispatchToDetector(ContactType type, Shape sharpA, Shape sharpB)
{
Debug.Assert(sharpA.Type <= sharpB.Type);
ParticleContact contact = null;
switch (type)
{
case ContactType.CircleAndCircle:
contact = ParticleCollisionDetector.CircleAndCircle(sharpA as Circle, sharpB as Circle);
break;
case ContactType.CircleAndEdge:
contact = ParticleCollisionDetector.CircleAndEdge(sharpA as Circle, sharpB as Edge);
break;
//case ContactType.CircleAndBox:
// throw new NotImplementedException("未实现圆与盒的碰撞检测");
//case ContactType.EdgeAndBox:
// throw new NotImplementedException("未实现边沿与盒的碰撞检测");
//case ContactType.BoxAndBox:
// throw new NotImplementedException("未实现盒与盒的碰撞检测");
}
return(contact);
}
public void TestResolveVelocityWithAcceleration()
{
var p = new Particle
{
Position = new Vector2D(0, 0),
Velocity = new Vector2D(1, 0),
Mass = 1
};
p.AddForce(new Vector2D(1, 0));
p.Update(1);
var contact = new ParticleContact(p, null, 1, 0, new Vector2D(-1, 0));
var resolveVelocity = new PrivateObject(contact);
resolveVelocity.Invoke("ResolveVelocity", 1);
Assert.AreEqual(new Vector2D(-1, 0), p.Velocity, "物体获得补偿速度");
p.AddForce(new Vector2D(1, 0));
p.Update(1);
Assert.AreEqual(new Vector2D(0, 0), p.Velocity, "物体保持静止");
p.AddForce(new Vector2D(-1, 0));
resolveVelocity.Invoke("ResolveVelocity", 1);
Assert.AreEqual(new Vector2D(0, 0), p.Velocity, "物体不会获得补偿速度");
}
public void fillParticleContacts(Particle particle, List <ParticleContact> contacts)
{
ParticleContact contact = default(ParticleContact);
for (int i = 0; i < 2; ++i)
{
if (particle.intersect(m_edges[i].normal, m_edges[i].d, ref contact))
{
contacts.Add(contact);
}
}
for (int i = 0; i < 4; ++i)
{
int b = i * 2;
if (intersectTwoRangeEdgesAndPoint(particle, s_rangEdgeIndices[b], s_rangEdgeIndices[b + 1], s_rangEdgeIndices[b], ref contact))
{
contacts.Add(contact);
}
}
var ball = particle as NewBallParticle;
if (ball != null)
{
//球
fillBallContacts(ball, contacts);
}
else
{
//人
if (((FBActor)particle.tag).configuration.bodyHeight >= m_doorHeight)
{
//人比球门高
for (int i = 0; i < m_ballEdges.Count; ++i)
{
if (particle.intersect(m_ballEdges[i].normal, m_ballEdges[i].d, ref contact))
{
contacts.Add(contact);
}
}
}
else
{
for (int i = 2; i < m_edges.Count; ++i)
{
if (particle.intersect(m_edges[i].normal, m_edges[i].d, ref contact))
{
switch (i)
{
case 2: contact.tagI = (int)(DoorSide.kDoor | DoorSide.kFieldA | DoorSide.kBack); break;
case 3: contact.tagI = (int)(DoorSide.kDoor | DoorSide.kFieldB | DoorSide.kBack); break;
}
contacts.Add(contact);
}
}
}
}
}
public void TestResolveVelocityWithTwoObject()
{
var p = new List <Particle>
{
new Particle
{
Position = new Vector2D(0, 0),
Velocity = new Vector2D(1, 0),
Mass = 1
},
new Particle
{
Position = new Vector2D(2, 0),
Velocity = new Vector2D(0, 0),
Mass = 1
}
};
var contact = new ParticleContact(p[0], p[1], 1, 1, new Vector2D(-1, 0));
var resolveVelocity = new PrivateObject(contact);
resolveVelocity.Invoke("ResolveVelocity", 1 / 60.0);
Assert.AreEqual(new Vector2D(0, 0), p[0].Velocity, "物体0失去速度");
Assert.AreEqual(new Vector2D(1, 0), p[1].Velocity, "物体1获得物体0碰撞前的速度");
}
//call all contact generators to report their contacts
// returns number of contacts
public int generateContacts()
{
int limit = maxContacts;
int i = 0;
ParticleContact nextContact = contacts[i];
//nextContact = this.thisContact;
while (i <= contactGenRegLen)
//while(i <= maxContacts)
{
int used = gen.addContact(nextContact, limit);
limit -= used;
i++;
if (contacts [i] != null)
{
nextContact = contacts [i];
}
//we've run out of contacts
if (limit <= 0)
{
break;
}
}
//MonoBehaviour.print("ParticleWorld: Contacts Generated");
//return number of contacts used
return(maxContacts - limit);
}
public int fillContact(ParticleContact contact, int limit)
{
//initializing Particle here because I can't do it above
particle = new MyParticle[2];
//find current length of the cable
float length = currentLength();
//check if over extended
if (length < maxLength)
{
return(0);
}
//otherwise return the contact
contact.particle [0] = particle [0];
contact.particle [1] = particle [1];
//calculate normal
MyVector3 normal = particle[1].getPosition().returnSubtractVectorFromThisVector(particle[0].getPosition());
normal.normalize();
contact.penetration = length - maxLength;
contact.restitution = restitution;
return(1);
}
void fillBallContacts(NewBallParticle ball, List <ParticleContact> contacts)
{
ParticleContact contact = default(ParticleContact);
if (ball.height >= m_doorHeight)
{
for (int i = 0; i < m_ballEdges.Count; ++i)
{
if (ball.intersect(m_ballEdges[i].normal, m_ballEdges[i].d, ref contact))
{
contacts.Add(contact);
}
}
}
else
{
#region jlx 2017.05.15-log:添加动态计算碰撞球门斜边
var dynamicSlopeEdge = calculateSlopeEdge(ball.position);
if (ball.intersect(dynamicSlopeEdge.normal, dynamicSlopeEdge.d, ref contact))
{
contact.tagI = (int)(
DoorSide.kDoor |
DoorSide.kBack |
(ball.position.x > Fix64.Zero ? DoorSide.kFieldA : DoorSide.kFieldB));
contacts.Add(contact);
}
#endregion
}
}
public void TestResolveInterpenetrationWithTwoFixedObject()
{
var p = new List <Particle>
{
new Particle
{
Position = new Vector2D(0, 0),
Velocity = new Vector2D(1, 0),
InverseMass = 0
},
new Particle
{
Position = new Vector2D(2, 0),
Velocity = new Vector2D(0, 0),
InverseMass = 0
}
};
var contact = new ParticleContact(p[0], p[1], 0.5, 1, new Vector2D(-1, 0));
var resolveVelocity = new PrivateObject(contact);
resolveVelocity.Invoke("ResolveInterpenetration", 1 / 60.0);
Assert.AreEqual(new Vector2D(0, 0), p[0].Position, "物体0位置不变");
Assert.AreEqual(new Vector2D(2, 0), p[1].Position, "物体1位置不变");
}
private void Start()
{
tr = GetComponent <TrailRenderer>();
startPos = new Vector3(transform.position.x, transform.position.y, transform.position.z);
throwableParticle = new Particle(new Vector3(startPos.x, startPos.y, startPos.z), 0.9f, 1f);
boundingRectangle = new BoundingRectangle(new Vector3(startPos.x, startPos.y, startPos.z), 1, 1);
particleContact = new ParticleContact(throwableParticle, 0f, new Vector3());
}
/// <summary>
/// 向碰撞表中添加一个新的碰撞
/// </summary>
/// <param name="contact">碰撞信息</param>
/// <returns>完成添加后若不允许继续添加则返回false，否则返回true</returns>
private bool AddToContactList(ParticleContact contact)
{
if (_contactCounter++ < Settings.MaxContacts)
{
_contactList.Add(contact);
return(true);
}
return(false);
}
/*
**Maybe**
*
* Add a particle contact list in here and add to that list with addContact.
* Create a particle contact generator and use the list created within that particle contact generator
*
*
**Alternative**
* add ParticleContact List as a parameter use .Add to add to it. TRY THIS FIRST!!!!!
*/
public int addContact(ParticleContact contact, int limit)
{
if (limit > 0)
{
contactReg.Add(contact);
return(1);
}
return(0);
}
public void TestResolveContacts()
{
var resolver = new ParticleContactResolver(100);
var p = new List <Particle>
{
new Particle
{
Position = new Vector2D(0, 0),
Velocity = new Vector2D(1, 0),
Mass = 1
},
new Particle
{
Position = new Vector2D(2, 0),
Velocity = new Vector2D(0, 0),
Mass = 1
}
};
var contact = new ParticleContact(p[0], p[1], 1, 1, new Vector2D(-1, 0));
var contactList = new List <ParticleContact>();
resolver.ResolveContacts(contactList, 1 / 60.0);
contactList.Add(contact);
resolver.ResolveContacts(contactList, 1 / 60.0);
Assert.AreEqual(new Vector2D(-1, 0), p[0].Position, "物体0依据速度分量分离");
Assert.AreEqual(new Vector2D(0, 0), p[0].Velocity, "物体0碰撞后速度相反");
Assert.AreEqual(new Vector2D(1, 0), p[1].Velocity, "物体1碰撞后速度相反");
p = new List <Particle>
{
new Particle
{
Position = new Vector2D(0, 0),
Velocity = new Vector2D(1, 0),
Mass = 1
},
new Particle
{
Position = new Vector2D(2, 0),
Velocity = new Vector2D(0, 0),
Mass = 1
}
};
contactList = new List <ParticleContact>
{
new ParticleContact(p[1], p[0], 1, 1, new Vector2D(1, 0))
};
resolver.ResolveContacts(contactList, 1 / 60.0);
Assert.AreEqual(new Vector2D(-1, 0), p[0].Position, "函数满足对称性");
Assert.AreEqual(new Vector2D(0, 0), p[0].Velocity, "函数满足对称性");
Assert.AreEqual(new Vector2D(1, 0), p[1].Velocity, "函数满足对称性");
}
ParticleContactResolver _onParticleCollided(Particle p1, Particle p2, ParticleContact contact)
{
var ball = p1 as NewBallParticle;
if (ball != null)
{
if (_checkContact((FBBall)ball.tag, (FBActor)p2.tag))
{
//ball.setCollidedEventFlag();
contact.normal = ajustActorBallCollideNormal(-contact.normal, (FBActor)p2.tag, ball.velocity);
return(new PCR_Obstacle()
{
particle = ball, contact = contact, restitution = config.ballCollisionRestitution_actorAndball
});
}
return(null);
}
ball = p2 as NewBallParticle;
if (ball != null)
{
if (_checkContact((FBBall)ball.tag, (FBActor)p1.tag))
{
//ball.setCollidedEventFlag();
contact.normal = ajustActorBallCollideNormal(contact.normal, (FBActor)p1.tag, ball.velocity);
return(new PCR_Obstacle()
{
particle = ball, contact = contact, restitution = config.ballCollisionRestitution_actorAndball
});
}
return(null);
}
FBActor actor = (FBActor)p1.tag;
if (actor != null && actor.ignoreCollision)
{
return(null);
}
FBActor actor2 = (FBActor)p2.tag;
if (actor2 != null && actor2.ignoreCollision)
{
return(null);
}
//碰撞数据保存
actor.onActorCollided(actor2);
actor2.onActorCollided(actor);
return(new PCR_Particle_DontMove_KeepVelocity()
{
p1 = p1, p2 = p2, contact = contact, restitution = config.ballCollisionRestitution_actorAndactor
});
}
void OnCollisionStay(Collision collision)
{
float penetrationDepth = 0;
if (collision.collider.CompareTag("Ground"))
{
ParticleContact pC = new ParticleContact(particle, null, restitution, collision.contacts [0].normal, penetrationDepth);
pC.Resolve(Time.fixedDeltaTime);
// Vector3 FrictionForce = new Vector3(particle.gravityForce.y * fritionFactor,0,0);
// particle.AddForce (FrictionForce);
}
}
public void TestCalculateSeparatingVelocityWithOneObject()
{
var p = new Particle
{
Position = new Vector2D(0, 0),
Velocity = new Vector2D(1, 0),
Mass = 1
};
var contact = new ParticleContact(p, null, 0.5, 1, new Vector2D(-1, 0));
Assert.AreEqual(-1, contact.CalculateSeparatingVelocity());
}
private static void TestDetectorsResult(
ParticleContact expectContact,
ParticleContact contact,
string message = "")
{
Assert.IsNotNull(contact, $"{message} 产生了碰撞");
Assert.AreEqual(expectContact.PA, contact.PA, $"{message} 物体A");
Assert.AreEqual(expectContact.PB, contact.PB, $"{message} 物体B");
Assert.AreEqual(expectContact.ContactNormal, contact.ContactNormal, $"{message} 碰撞法线");
Assert.AreEqual(expectContact.Restitution, contact.Restitution, $"{message} 回弹系数");
Assert.AreEqual(expectContact.Penetration, contact.Penetration, $"{message} 相交深度");
}
void setDoorSide(int index, ref ParticleContact contact)
{
switch (index)
{
case 4: contact.tagI = (int)(DoorSide.kDoor | DoorSide.kFieldB | DoorSide.kLeft); break;
case 5: contact.tagI = (int)(DoorSide.kDoor | DoorSide.kFieldB | DoorSide.kRight); break;
case 6: contact.tagI = (int)(DoorSide.kDoor | DoorSide.kFieldA | DoorSide.kRight); break;
case 7: contact.tagI = (int)(DoorSide.kDoor | DoorSide.kFieldA | DoorSide.kLeft); break;
}
}
/// <summary>
/// Creates a new particle simulator that can handle up to the
/// given number of contacts per frame.You can also optionally
/// give a number of contact-resolution iterations to use. If you
/// don't give a number of iterations, then twice the number of
/// contacts will be used.
/// </summary>
public ParticleEngine(int maxContacts)
{
Particles = new List <Particle>();
ForceAreas = new List <ParticleForceArea>();
Forces = new List <ParticleForce>();
Constraints = new List <ParticleConstraint>();
Resolver = new ParticleContactResolver();
m_contacts = new ParticleContact[maxContacts];
for (int i = 0; i < maxContacts; i++)
{
m_contacts[i] = new ParticleContact();
}
}
public void onCollided(ParticleContact contact)
{
m_particle.notify_collided();
if (m_willBeCatched)
{
m_willBeCatched = false;
m_willBeCatchedActor = null;
}
else
{
ballState = BallState.Free;
}
collidedContact = contact;
collidedVelocity = get3DVelocity();
}
public void TestResolveVelocityWithOneObject()
{
var p = new Particle
{
Position = new Vector2D(0, 0),
Velocity = new Vector2D(1, 0),
Mass = 1
};
var contact = new ParticleContact(p, null, 1, 1, new Vector2D(-1, 0));
var resolveVelocity = new PrivateObject(contact);
resolveVelocity.Invoke("ResolveVelocity", 1 / 60.0);
Assert.AreEqual(new Vector2D(-1, 0), p.Velocity);
}
void Start()
{
Vector3 startPos = new Vector3(transform.position.x, transform.position.y, transform.position.z);
eventPoint = new Vector3(100f, -5.1f, 0);
particle = new Particle(new Vector3(startPos.x, startPos.y, startPos.z), 0.8f, 1f);
boundingRectangle = new BoundingRectangle(new Vector3(startPos.x, startPos.y, startPos.z), 1.8f, 2.8f);
particleContact = new ParticleContact(particle, 0f, new Vector3());
Scene scene = SceneManager.GetActiveScene();
string[] LevelsWithSuit = System.Enum.GetNames(typeof(GameManager.LevelsWithSuit));
foreach (string level in LevelsWithSuit)
{
if (scene.name == level)
{
usableSuit = true;
}
}
}
void OnCollisionEnter(Collision collision)
{
if (collision.collider.CompareTag("Ground"))
{
ParticleContact pC = new ParticleContact(particle, null, restitution, collision.contacts [0].normal, 0);
pC.Resolve(Time.fixedDeltaTime);
}
else if (collision.collider.CompareTag("wall"))
{
ParticleContact pC = new ParticleContact(particle, null, restitution, collision.contacts [0].normal, 0);
pC.Resolve(Time.fixedDeltaTime);
Debug.Log(gameObject.transform.position);
}
else
{
Particle other = collision.collider.GetComponent <MyPhysics> ().particle;
ParticleContact pC = new ParticleContact(particle, other, restitution, collision.contacts [0].normal, 0);
pC.Resolve(Time.fixedDeltaTime);
}
}
public void TestCalculateSeparatingVelocityWithTwoObject()
{
var p = new List <Particle>
{
new Particle
{
Position = new Vector2D(0, 0),
Velocity = new Vector2D(1, 0),
Mass = 1
},
new Particle
{
Position = new Vector2D(2, 0),
Velocity = new Vector2D(0, 0),
Mass = 1
}
};
var contact = new ParticleContact(p[0], p[1], 0.5, 1, new Vector2D(-1, 0));
Assert.AreEqual(-1, contact.CalculateSeparatingVelocity());
}
//page 149 millington pdf
//fils structure with given contact needed to prevent it from violating its constraint
public int fillContact(ParticleContact contact, int limit)
{
particle = new MyParticle[2];
//find current length of the rod
float currentLen = currentLength();
//check whether we're overextended
if (currentLen == length)
{
return(0);
}
//otherwise return the contact
contact.particle [0] = particle [0];
contact.particle [1] = particle [1];
//calculate normal
MyVector3 normal = particle[1].getPosition().returnSubtractVectorFromThisVector(particle[0].getPosition());
normal.normalize();
//contact normal is depended on whether or not we're extending or compressing
if (currentLen > length)
{
contact.contactNormal = normal;
contact.penetration = currentLen - length;
}
else
{
contact.contactNormal = normal.multiplyVectorByScalarVR(-1);
contact.penetration = length - currentLen;
}
//no bounciness so restitution is always zero
contact.restitution = 0;
return(1);
}
public override bool AddContact(ParticleContact contact)
{
bool ret = PhysicsEngineForFunPINVOKE.ParticleCable_AddContact(swigCPtr, ParticleContact.getCPtr(contact));
return(ret);
}
ParticleContactResolver _onObstacleCollided(Particle particle, Obstacle obstacle, ParticleContact contact)
{
var ball = particle as NewBallParticle;
if (ball != null)
{
((FBBall)ball.tag).onCollided(contact);
if (contact.tagI == (int)ArenaObstacle.DoorSide.kOutDoor)
{
return(new PCR_Obstacle()
{
particle = ball, obstacle = obstacle, contact = contact, restitution = config.ballCollisionRestitution[0]
});
}
PCR_Obstacle pcrObstracle = new PCR_Obstacle();
pcrObstracle.particle = ball;
pcrObstracle.obstacle = obstacle;
pcrObstracle.contact = contact;
if ((contact.tagI & (int)ArenaObstacle.DoorSide.kBack) == (int)ArenaObstacle.DoorSide.kBack)
{
//后面
pcrObstracle.restitution = config.ballCollisionRestitution[1];
}
else if ((contact.tagI & (int)ArenaObstacle.DoorSide.kLeft) == (int)ArenaObstacle.DoorSide.kLeft)
{
//左侧边
pcrObstracle.restitution = config.ballCollisionRestitution[2];
}
else if ((contact.tagI & (int)ArenaObstacle.DoorSide.kRight) == (int)ArenaObstacle.DoorSide.kRight)
{
//右侧边
pcrObstracle.restitution = config.ballCollisionRestitution[3];
}
else if ((contact.tagI & (int)ArenaObstacle.DoorSide.kCeil) == (int)ArenaObstacle.DoorSide.kCeil)
{
//天花板
pcrObstracle.restitution = config.ballCollisionRestitution[4];
}
return(pcrObstracle);
}
return(new PCR_Obstacle_DontMove_KeepVelocity()
{
particle = particle, obstacle = obstacle, contact = contact, restitution = config.ballCollisionRestitution_actorAndobstacle
});
}
internal static global::System.Runtime.InteropServices.HandleRef getCPtr(ParticleContact obj)
{
return((obj == null) ? new global::System.Runtime.InteropServices.HandleRef(null, global::System.IntPtr.Zero) : obj.swigCPtr);
}
public virtual bool AddContact(ParticleContact contact)
{
bool ret = (SwigDerivedClassHasMethod("AddContact", swigMethodTypes0) ? PhysicsEngineForFunPINVOKE.IParticleContactGenerator_AddContactSwigExplicitIParticleContactGenerator(swigCPtr, ParticleContact.getCPtr(contact)) : PhysicsEngineForFunPINVOKE.IParticleContactGenerator_AddContact(swigCPtr, ParticleContact.getCPtr(contact)));
return(ret);
}
